static int applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t adjustmentFunction, int delta) { beeperConfirmationBeeps(delta > 0 ? 2 : 1); int newValue; switch (adjustmentFunction) { case ADJUSTMENT_RC_RATE: case ADJUSTMENT_ROLL_RC_RATE: newValue = constrain((int)controlRateConfig->rcRates[FD_ROLL] + delta, 0, CONTROL_RATE_CONFIG_RC_RATES_MAX); controlRateConfig->rcRates[FD_ROLL] = newValue; blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_RC_RATE, newValue); if (adjustmentFunction == ADJUSTMENT_ROLL_RC_RATE) { break; } // fall through for combined ADJUSTMENT_RC_EXPO FALLTHROUGH; case ADJUSTMENT_PITCH_RC_RATE: newValue = constrain((int)controlRateConfig->rcRates[FD_PITCH] + delta, 0, CONTROL_RATE_CONFIG_RC_RATES_MAX); controlRateConfig->rcRates[FD_PITCH] = newValue; blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_RC_RATE, newValue); break; case ADJUSTMENT_RC_EXPO: case ADJUSTMENT_ROLL_RC_EXPO: newValue = constrain((int)controlRateConfig->rcExpo[FD_ROLL] + delta, 0, CONTROL_RATE_CONFIG_RC_EXPO_MAX); controlRateConfig->rcExpo[FD_ROLL] = newValue; blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_RC_EXPO, newValue); if (adjustmentFunction == ADJUSTMENT_ROLL_RC_EXPO) { break; } // fall through for combined ADJUSTMENT_RC_EXPO FALLTHROUGH; case ADJUSTMENT_PITCH_RC_EXPO: newValue = constrain((int)controlRateConfig->rcExpo[FD_PITCH] + delta, 0, CONTROL_RATE_CONFIG_RC_EXPO_MAX); controlRateConfig->rcExpo[FD_PITCH] = newValue; blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_RC_EXPO, newValue); break; case ADJUSTMENT_THROTTLE_EXPO: newValue = constrain((int)controlRateConfig->thrExpo8 + delta, 0, 100); // FIXME magic numbers repeated in cli.c controlRateConfig->thrExpo8 = newValue; initRcProcessing(); blackboxLogInflightAdjustmentEvent(ADJUSTMENT_THROTTLE_EXPO, newValue); break; case ADJUSTMENT_PITCH_ROLL_RATE: case ADJUSTMENT_PITCH_RATE: newValue = constrain((int)controlRateConfig->rates[FD_PITCH] + delta, 0, CONTROL_RATE_CONFIG_RATE_MAX); controlRateConfig->rates[FD_PITCH] = newValue; blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_RATE, newValue); if (adjustmentFunction == ADJUSTMENT_PITCH_RATE) { break; } // fall through for combined ADJUSTMENT_PITCH_ROLL_RATE FALLTHROUGH; case ADJUSTMENT_ROLL_RATE: newValue = constrain((int)controlRateConfig->rates[FD_ROLL] + delta, 0, CONTROL_RATE_CONFIG_RATE_MAX); controlRateConfig->rates[FD_ROLL] = newValue; blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_RATE, newValue); break; case ADJUSTMENT_YAW_RATE: newValue = constrain((int)controlRateConfig->rates[FD_YAW] + delta, 0, CONTROL_RATE_CONFIG_RATE_MAX); controlRateConfig->rates[FD_YAW] = newValue; blackboxLogInflightAdjustmentEvent(ADJUSTMENT_YAW_RATE, newValue); break; case ADJUSTMENT_PITCH_ROLL_P: case ADJUSTMENT_PITCH_P: newValue = constrain((int)pidProfile->pid[PID_PITCH].P + delta, 0, 200); // FIXME magic numbers repeated in cli.c pidProfile->pid[PID_PITCH].P = newValue; blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_P, newValue); if (adjustmentFunction == ADJUSTMENT_PITCH_P) { break; } // fall through for combined ADJUSTMENT_PITCH_ROLL_P FALLTHROUGH; case ADJUSTMENT_ROLL_P: newValue = constrain((int)pidProfile->pid[PID_ROLL].P + delta, 0, 200); // FIXME magic numbers repeated in cli.c pidProfile->pid[PID_ROLL].P = newValue; blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_P, newValue); break; case ADJUSTMENT_PITCH_ROLL_I: case ADJUSTMENT_PITCH_I: newValue = constrain((int)pidProfile->pid[PID_PITCH].I + delta, 0, 200); // FIXME magic numbers repeated in cli.c pidProfile->pid[PID_PITCH].I = newValue; blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_I, newValue); if (adjustmentFunction == ADJUSTMENT_PITCH_I) { break; } // fall through for combined ADJUSTMENT_PITCH_ROLL_I FALLTHROUGH; case ADJUSTMENT_ROLL_I: newValue = constrain((int)pidProfile->pid[PID_ROLL].I + delta, 0, 200); // FIXME magic numbers repeated in cli.c pidProfile->pid[PID_ROLL].I = newValue; blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_I, newValue); break; case ADJUSTMENT_PITCH_ROLL_D: case ADJUSTMENT_PITCH_D: newValue = constrain((int)pidProfile->pid[PID_PITCH].D + delta, 0, 200); // FIXME magic numbers repeated in cli.c pidProfile->pid[PID_PITCH].D = newValue; blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_D, newValue); if (adjustmentFunction == ADJUSTMENT_PITCH_D) { break; } // fall through for combined ADJUSTMENT_PITCH_ROLL_D FALLTHROUGH; case ADJUSTMENT_ROLL_D: newValue = constrain((int)pidProfile->pid[PID_ROLL].D + delta, 0, 200); // FIXME magic numbers repeated in cli.c pidProfile->pid[PID_ROLL].D = newValue; blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_D, newValue); break; case ADJUSTMENT_YAW_P: newValue = constrain((int)pidProfile->pid[PID_YAW].P + delta, 0, 200); // FIXME magic numbers repeated in cli.c pidProfile->pid[PID_YAW].P = newValue; blackboxLogInflightAdjustmentEvent(ADJUSTMENT_YAW_P, newValue); break; case ADJUSTMENT_YAW_I: newValue = constrain((int)pidProfile->pid[PID_YAW].I + delta, 0, 200); // FIXME magic numbers repeated in cli.c pidProfile->pid[PID_YAW].I = newValue; blackboxLogInflightAdjustmentEvent(ADJUSTMENT_YAW_I, newValue); break; case ADJUSTMENT_YAW_D: newValue = constrain((int)pidProfile->pid[PID_YAW].D + delta, 0, 200); // FIXME magic numbers repeated in cli.c pidProfile->pid[PID_YAW].D = newValue; blackboxLogInflightAdjustmentEvent(ADJUSTMENT_YAW_D, newValue); break; case ADJUSTMENT_RC_RATE_YAW: newValue = constrain((int)controlRateConfig->rcRates[FD_YAW] + delta, 0, CONTROL_RATE_CONFIG_RC_RATES_MAX); controlRateConfig->rcRates[FD_YAW] = newValue; blackboxLogInflightAdjustmentEvent(ADJUSTMENT_RC_RATE_YAW, newValue); break; case ADJUSTMENT_D_SETPOINT: newValue = constrain((int)pidProfile->dtermSetpointWeight + delta, 0, 254); // FIXME magic numbers repeated in cli.c pidProfile->dtermSetpointWeight = newValue; blackboxLogInflightAdjustmentEvent(ADJUSTMENT_D_SETPOINT, newValue); break; case ADJUSTMENT_D_SETPOINT_TRANSITION: newValue = constrain((int)pidProfile->setpointRelaxRatio + delta, 1, 100); // FIXME magic numbers repeated in cli.c pidProfile->setpointRelaxRatio = newValue; blackboxLogInflightAdjustmentEvent(ADJUSTMENT_D_SETPOINT_TRANSITION, newValue); break; default: newValue = -1; break; }; return newValue; }
static void H2R_HSBtoRGB(float hue, float sat, float bright, float *color) { // constrain all input variables to expected range hue = constrain(hue, 0, 360); sat = constrain(sat, 0, 100); bright = constrain(bright, 0, 100); // define maximum value for RGB array elements float max_rgb_val = H2R_MAX_RGB_val; // convert saturation and brightness value to decimals and init r, g, b variables float sat_f = (float)sat / 100.0; float bright_f = (float)bright / 100.0; float r, g, b; // If brightness is 0 then color is black (achromatic) // therefore, R, G and B values will all equal to 0 if (bright <= 0) { color[0] = 0; color[1] = 0; color[2] = 0; } // If saturation is 0 then color is gray (achromatic) // therefore, R, G and B values will all equal the current brightness if (sat <= 0) { color[0] = bright_f * max_rgb_val; color[1] = bright_f * max_rgb_val; color[2] = bright_f * max_rgb_val; } // if saturation and brightness are greater than 0 then calculate // R, G and B values based on the current hue and brightness else { if (hue >= 0 && hue < 120) { float hue_primary = 1.0 - ((float)hue / 120.0); float hue_secondary = (float)hue / 120.0; float sat_primary = (1.0 - hue_primary) * (1.0 - sat_f); float sat_secondary = (1.0 - hue_secondary) * (1.0 - sat_f); float sat_tertiary = 1.0 - sat_f; r = (bright_f * max_rgb_val) * (hue_primary + sat_primary); g = (bright_f * max_rgb_val) * (hue_secondary + sat_secondary); b = (bright_f * max_rgb_val) * sat_tertiary; } else if (hue >= 120 && hue < 240) { float hue_primary = 1.0 - (((float)hue-120.0) / 120.0); float hue_secondary = ((float)hue-120.0) / 120.0; float sat_primary = (1.0 - hue_primary) * (1.0 - sat_f); float sat_secondary = (1.0 - hue_secondary) * (1.0 - sat_f); float sat_tertiary = 1.0 - sat_f; r = (bright_f * max_rgb_val) * sat_tertiary; g = (bright_f * max_rgb_val) * (hue_primary + sat_primary); b = (bright_f * max_rgb_val) * (hue_secondary + sat_secondary); } else if (hue >= 240 && hue <= 360) { float hue_primary = 1.0 - (((float)hue-240.0) / 120.0); float hue_secondary = ((float)hue-240.0) / 120.0; float sat_primary = (1.0 - hue_primary) * (1.0 - sat_f); float sat_secondary = (1.0 - hue_secondary) * (1.0 - sat_f); float sat_tertiary = 1.0 - sat_f; r = (bright_f * max_rgb_val) * (hue_secondary + sat_secondary); g = (bright_f * max_rgb_val) * sat_tertiary; b = (bright_f * max_rgb_val) * (hue_primary + sat_primary); } color[0] = r; color[1] = g; color[2] = b; } // color[0] = Percent(color[0]); // color[1] = Percent(color[1]); // color[2] = Percent(color[2]); }
static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig) { float RateError, errorAngle, AngleRate, gyroRate; float ITerm,PTerm,DTerm; int32_t stickPosAil, stickPosEle, mostDeflectedPos; static float lastGyroRate[3]; static float delta1[3], delta2[3]; float delta, deltaSum; float dT; int axis; float horizonLevelStrength = 1; dT = (float)cycleTime * 0.000001f; if (FLIGHT_MODE(HORIZON_MODE)) { // Figure out the raw stick positions stickPosAil = getRcStickDeflection(FD_ROLL, rxConfig->midrc); stickPosEle = getRcStickDeflection(FD_PITCH, rxConfig->midrc); if(ABS(stickPosAil) > ABS(stickPosEle)){ mostDeflectedPos = ABS(stickPosAil); } else { mostDeflectedPos = ABS(stickPosEle); } // Progressively turn off the horizon self level strength as the stick is banged over horizonLevelStrength = (float)(500 - mostDeflectedPos) / 500; // 1 at centre stick, 0 = max stick deflection if(pidProfile->H_sensitivity == 0){ horizonLevelStrength = 0; } else { horizonLevelStrength = constrainf(((horizonLevelStrength - 1) * (100 / pidProfile->H_sensitivity)) + 1, 0, 1); } } // ----------PID controller---------- for (axis = 0; axis < 3; axis++) { // -----Get the desired angle rate depending on flight mode uint8_t rate = controlRateConfig->rates[axis]; if (axis == FD_YAW) { // YAW is always gyro-controlled (MAG correction is applied to rcCommand) 100dps to 1100dps max yaw rate AngleRate = (float)((rate + 10) * rcCommand[YAW]) / 50.0f; } else { // calculate error and limit the angle to the max inclination #ifdef GPS errorAngle = (constrain(rcCommand[axis] + GPS_angle[axis], -((int) max_angle_inclination), +max_angle_inclination) - inclination.raw[axis] + angleTrim->raw[axis]) / 10.0f; // 16 bits is ok here #else errorAngle = (constrain(rcCommand[axis], -((int) max_angle_inclination), +max_angle_inclination) - inclination.raw[axis] + angleTrim->raw[axis]) / 10.0f; // 16 bits is ok here #endif #ifdef AUTOTUNE if (shouldAutotune()) { errorAngle = autotune(rcAliasToAngleIndexMap[axis], &inclination, errorAngle); } #endif if (FLIGHT_MODE(ANGLE_MODE)) { // it's the ANGLE mode - control is angle based, so control loop is needed AngleRate = errorAngle * pidProfile->A_level; } else { //control is GYRO based (ACRO and HORIZON - direct sticks control is applied to rate PID AngleRate = (float)((rate + 20) * rcCommand[axis]) / 50.0f; // 200dps to 1200dps max yaw rate if (FLIGHT_MODE(HORIZON_MODE)) { // mix up angle error to desired AngleRate to add a little auto-level feel AngleRate += errorAngle * pidProfile->H_level * horizonLevelStrength; } } } gyroRate = gyroData[axis] * gyro.scale; // gyro output scaled to dps // --------low-level gyro-based PID. ---------- // Used in stand-alone mode for ACRO, controlled by higher level regulators in other modes // -----calculate scaled error.AngleRates // multiplication of rcCommand corresponds to changing the sticks scaling here RateError = AngleRate - gyroRate; // -----calculate P component PTerm = RateError * pidProfile->P_f[axis]; // -----calculate I component errorGyroIf[axis] = constrainf(errorGyroIf[axis] + RateError * dT * pidProfile->I_f[axis], -250.0f, 250.0f); // limit maximum integrator value to prevent WindUp - accumulating extreme values when system is saturated. // I coefficient (I8) moved before integration to make limiting independent from PID settings ITerm = errorGyroIf[axis]; //-----calculate D-term delta = gyroRate - lastGyroRate[axis]; // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800 lastGyroRate[axis] = gyroRate; // Correct difference by cycle time. Cycle time is jittery (can be different 2 times), so calculated difference // would be scaled by different dt each time. Division by dT fixes that. delta *= (1.0f / dT); // add moving average here to reduce noise deltaSum = delta1[axis] + delta2[axis] + delta; delta2[axis] = delta1[axis]; delta1[axis] = delta; DTerm = constrainf((deltaSum / 3.0f) * pidProfile->D_f[axis], -300.0f, 300.0f); // -----calculate total PID output axisPID[axis] = constrain(lrintf(PTerm + ITerm - DTerm), -1000, 1000); #ifdef BLACKBOX axisPID_P[axis] = PTerm; axisPID_I[axis] = ITerm; axisPID_D[axis] = -DTerm; #endif } }
static void stabilizerAltHoldUpdate(void) { // Get altitude hold commands from pilot commanderGetAltHold(&altHold, &setAltHold, &altHoldChange); // Get barometer height estimates //TODO do the smoothing within getData #ifdef PLATFORM_CF1 ms5611GetData(&pressure, &temperature, &aslRaw); #else lps25hGetData(&pressure, &temperature, &aslRaw); #endif asl = asl * aslAlpha + aslRaw * (1 - aslAlpha); aslLong = aslLong * aslAlphaLong + aslRaw * (1 - aslAlphaLong); // Estimate vertical speed based on successive barometer readings. This is ugly :) vSpeedASL = deadband(asl - aslLong, vSpeedASLDeadband); // Estimate vertical speed based on Acc - fused with baro to reduce drift vSpeed = constrain(vSpeed, -vSpeedLimit, vSpeedLimit); vSpeed = vSpeed * vBiasAlpha + vSpeedASL * (1.f - vBiasAlpha); vSpeedAcc = vSpeed; // Reset Integral gain of PID controller if being charged if (!pmIsDischarging()) { altHoldPID.integ = 0.0; } // Altitude hold mode just activated, set target altitude as current altitude. Reuse previous integral term as a starting point if (setAltHold) { // Set to current altitude altHoldTarget = asl; // Cache last integral term for reuse after pid init const float pre_integral = altHoldPID.integ; // Reset PID controller pidInit(&altHoldPID, asl, altHoldKp, altHoldKi, altHoldKd, ALTHOLD_UPDATE_DT); // TODO set low and high limits depending on voltage // TODO for now just use previous I value and manually set limits for whole voltage range // pidSetIntegralLimit(&altHoldPID, 12345); // pidSetIntegralLimitLow(&altHoldPID, 12345); / altHoldPID.integ = pre_integral; // Reset altHoldPID altHoldPIDVal = pidUpdate(&altHoldPID, asl, false); } // In altitude hold mode if (altHold) { // Update target altitude from joy controller input altHoldTarget += altHoldChange / altHoldChange_SENS; pidSetDesired(&altHoldPID, altHoldTarget); // Compute error (current - target), limit the error altHoldErr = constrain(deadband(asl - altHoldTarget, errDeadband), -altHoldErrMax, altHoldErrMax); pidSetError(&altHoldPID, -altHoldErr); // Get control from PID controller, dont update the error (done above) // Smooth it and include barometer vspeed // TODO same as smoothing the error?? altHoldPIDVal = (pidAlpha) * altHoldPIDVal + (1.f - pidAlpha) * ((vSpeedAcc * vSpeedAccFac) + (vSpeedASL * vSpeedASLFac) + pidUpdate(&altHoldPID, asl, false)); // compute new thrust actuatorThrust = max(altHoldMinThrust, min(altHoldMaxThrust, limitThrust( altHoldBaseThrust + (int32_t)(altHoldPIDVal*pidAslFac)))); // i part should compensate for voltage drop } else { altHoldTarget = 0.0; altHoldErr = 0.0; altHoldPIDVal = 0.0; } }
float PID::get_p_error_lim(float targetInput, float actualInput){ _error = targetInput - actualInput; _error = constrain(_error,-_pMax._value,_pMax._value); return _error; }
void LinearActuator::goToPosition(uint16_t target_position) { _target_position = constrain(target_position, LA_RAW_POS_MIN, LA_RAW_POS_MAX); }
float Ramp::progress() const { // Compute progress as % of duration. float prog = elapsed() / _duration; prog = constrain(prog, 0, 1); return prog; }
/************************************************************************** * \brief Filter DCM Drift Correction * * \param --- * * \return --- ***************************************************************************/ static void filter_dcm_drift_correction(void) { //Compensation the Roll, Pitch and Yaw drift. //float mag_heading_x; //float mag_heading_y; //static float Scaled_Omega_P[3]; //float Integrator_magnitude; //float tempfloat; static float Scaled_Omega_I[3]; float Accel_magnitude; float Accel_weight; //***** Roll and Pitch *************** /* Calculate the magnitude of the accelerometer vector */ Accel_magnitude = sqrt(Accel_Vector[0]*Accel_Vector[0] + Accel_Vector[1]*Accel_Vector[1] + Accel_Vector[2]*Accel_Vector[2]); //Accel_magnitude = Accel_magnitude/4096; // Scale to gravity. /* Dynamic weighting of accelerometer info (reliability filter) * Weight for accelerometer info (<0.5G = 0.0, 1G = 1.0 , >1.5G = 0.0) */ Accel_weight = constrain(1 - 2*abs(1 - Accel_magnitude),0,1); // Vector_Cross_Product(&errorRollPitch[0],&Accel_Vector[0],&DCM_Matrix[2][0]); //adjust the ground of reference Vector_Scale(&Omega_P[0],&errorRollPitch[0],filterdata->Kp_rollPitch*Accel_weight); Vector_Scale(&Scaled_Omega_I[0],&errorRollPitch[0],filterdata->Ki_rollPitch*Accel_weight); Vector_Add(Omega_I,Omega_I,Scaled_Omega_I); /*********** YAW ***************/ //#if FILTER_USE_MAGNETOMETER==1 //// We make the gyro YAW drift correction based on compass magnetic heading //#if BOARD_VERSION < 3 //errorCourse=(DCM_Matrix[0][0]*APM_Compass.Heading_Y) - (DCM_Matrix[1][0]*APM_Compass.Heading_X); //Calculating YAW error //#endif //#if BOARD_VERSION == 3 //errorCourse=(DCM_Matrix[0][0]*Heading_Y) - (DCM_Matrix[1][0]*Heading_X); //Calculating YAW error //#endif //Vector_Scale(errorYaw,&DCM_Matrix[2][0],errorCourse); //Applys the yaw correction to the XYZ rotation of the aircraft, depeding the position. // //Vector_Scale(&Scaled_Omega_P[0],&errorYaw[0],Kp_YAW); //Vector_Add(Omega_P,Omega_P,Scaled_Omega_P);//Adding Proportional. // //Vector_Scale(&Scaled_Omega_I[0],&errorYaw[0],Ki_YAW); //Vector_Add(Omega_I,Omega_I,Scaled_Omega_I);//adding integrator to the Omega_I //#else // Use GPS Ground course to correct yaw gyro drift //if(GPS.ground_speed>=SPEEDFILT*100) // Ground speed from GPS is in m/s //{ //COGX = cos(ToRad(GPS.ground_course/100.0)); //COGY = sin(ToRad(GPS.ground_course/100.0)); //errorCourse=(DCM_Matrix[0][0]*COGY) - (DCM_Matrix[1][0]*COGX); //Calculating YAW error //Vector_Scale(errorYaw,&DCM_Matrix[2][0],errorCourse); //Applys the yaw correction to the XYZ rotation of the aircraft, depeding the position. // //Vector_Scale(&Scaled_Omega_P[0],&errorYaw[0],Kp_YAW); //Vector_Add(Omega_P,Omega_P,Scaled_Omega_P);//Adding Proportional. // //Vector_Scale(&Scaled_Omega_I[0],&errorYaw[0],Ki_YAW); //Vector_Add(Omega_I,Omega_I,Scaled_Omega_I);//adding integrator to the Omega_I //} //#endif //// Here we will place a limit on the integrator so that the integrator cannot ever exceed half the saturation limit of the gyros //Integrator_magnitude = sqrt(Vector_Dot_Product(Omega_I,Omega_I)); //if (Integrator_magnitude > ToRad(300)) { //Vector_Scale(Omega_I,Omega_I,0.5f*ToRad(300)/Integrator_magnitude); //} }
float ray2::get_closest_t_constrained(const vec2& p) const { return constrain(0.f, 1.f, get_closest_t_unconstrained(p)); }
/******************************************************************************* ** Main Function main() *******************************************************************************/ int main (void) { /* Basic chip initialization is taken care of in SystemInit() called * from the startup code. SystemInit() and chip settings are defined * in the CMSIS system_<part family>.c file. */ setup(); static uint8_t rcDelayCommand; // this indicates the number of time (multiple of RC measurement at 50Hz) the sticks must be maintained to run or switch off motors uint8_t axis,i; int16_t delta,deltaSum; int16_t PTerm,ITerm,DTerm; static int16_t lastGyro[3] = {0,0,0}; static int16_t delta1[3],delta2[3]; static int16_t errorAngleI[2] = {0,0}; static uint32_t rcTime = 0; static int16_t initialThrottleHold; int16_t error,errorAngle =0; #if PIDcontroller ==1 static int16_t errorGyroI[3] = {0,0,0}; #elif PIDcontroller == 2 int32_t AngleRateTmp = 0, RateError = 0, AngleITerm = 0; static int32_t errorGyroI[3] = {0,0,0}; #endif #define RC_FREQ 50 while(1) { #ifdef LCD_TELEMETRY_STEP static char telemetryStepSequence [] = LCD_TELEMETRY_STEP; static uint8_t telemetryStepIndex = 0; #endif #if defined(SPEKTRUM) if (rcFrameComplete) computeRC(); #endif #if defined(OPENLRSv2MULTI) Read_OpenLRS_RC(); #endif if ((currentTime > rcTime) || rcTime > (currentTime + 20000)) { // 50Hz + additional check to ensure rcTime can reach currentTime rcTime = currentTime + 20000; computeRC(); //reset watchdog timer in RC loop to ensure user is not locked out of control feedWatchDog(); // Failsafe routine - added by MIS #if defined(FAILSAFE) if ( failsafeCnt > (5*FAILSAVE_DELAY) && f.ARMED) { // Stabilize, and set Throttle to specified level for(i=0; i<3; i++) rcData[i] = MIDRC; // after specified guard time after RC signal is lost (in 0.1sec) rcData[THROTTLE] = FAILSAVE_THROTTLE; if (failsafeCnt > 5*(FAILSAVE_DELAY+FAILSAVE_OFF_DELAY)) { // Turn OFF motors after specified Time (in 0.1sec) f.ARMED = 0; // This will prevent the copter to automatically rearm if failsafe shuts it down and prevents f.OK_TO_ARM = 0; // to restart accidentely by just reconnect to the tx - you will have to switch off first to rearm } failsafeEvents++; } if ( failsafeCnt > (5*FAILSAVE_DELAY) && !f.ARMED) { //Turn of "Ok To arm to prevent the motors from spinning after repowering the RX with low throttle and aux to arm f.ARMED = 0; // This will prevent the copter to automatically rearm if failsafe shuts it down and prevents f.OK_TO_ARM = 0; // to restart accidentely by just reconnect to the tx - you will have to switch off first to rearm } failsafeCnt++; #endif // end of failsave routine - next change is made with RcOptions setting if (rcData[THROTTLE] < MINCHECK) { errorGyroI[ROLL] = 0; errorGyroI[PITCH] = 0; errorGyroI[YAW] = 0; errorAngleI[ROLL] = 0; errorAngleI[PITCH] = 0; rcDelayCommand++; if (rcData[YAW] < MINCHECK && rcData[PITCH] < MINCHECK && !f.ARMED) { if (rcDelayCommand == 20) { calibratingG=400; #if GPS GPS_reset_home_position(); #endif } } else if (rcData[YAW] > MAXCHECK && rcData[PITCH] > MAXCHECK && !f.ARMED) { if (rcDelayCommand == 20) { #ifdef TRI servo[5] = 1500; // we center the yaw servo in conf mode writeServos(); #endif #ifdef FLYING_WING servo[0] = conf.wing_left_mid; servo[1] = conf.wing_right_mid; writeServos(); #endif #ifdef AIRPLANE for(i = 4; i<7 ;i++) servo[i] = 1500; writeServos(); #endif #if defined(LCD_CONF) configurationLoop(); // beginning LCD configuration #endif previousTime = micros(); } } #if defined(INFLIGHT_ACC_CALIBRATION) else if (!f.ARMED && rcData[YAW] < MINCHECK && rcData[PITCH] > MAXCHECK && rcData[ROLL] > MAXCHECK){ if (rcDelayCommand == 20){ if (AccInflightCalibrationMeasurementDone){ // trigger saving into eeprom after landing AccInflightCalibrationMeasurementDone = 0; AccInflightCalibrationSavetoEEProm = 1; }else{ AccInflightCalibrationArmed = !AccInflightCalibrationArmed; #if defined(BUZZER) if (AccInflightCalibrationArmed){ toggleBeep = 2; } else { toggleBeep = 3; } #endif } } } #endif else if (conf.activate[BOXARM] > 0) { if ( rcOptions[BOXARM] && f.OK_TO_ARM #if defined(FAILSAFE) && failsafeCnt <= 1 #endif ) { f.ARMED = 1; headFreeModeHold = heading; } else if (f.ARMED) f.ARMED = 0; rcDelayCommand = 0; #ifdef ALLOW_ARM_DISARM_VIA_TX_YAW } else if ( (rcData[YAW] < MINCHECK ) && f.ARMED) { if (rcDelayCommand == 20) f.ARMED = 0; // rcDelayCommand = 20 => 20x20ms = 0.4s = time to wait for a specific RC command to be acknowledged } else if ( (rcData[YAW] > MAXCHECK ) && rcData[PITCH] < MAXCHECK && !f.ARMED && calibratingG == 0 && f.ACC_CALIBRATED) { if (rcDelayCommand == 20) { f.ARMED = 1; headFreeModeHold = heading; } #endif #ifdef ALLOW_ARM_DISARM_VIA_TX_ROLL } else if ( (rcData[ROLL] < MINCHECK) && f.ARMED) { if (rcDelayCommand == 20) f.ARMED = 0; // rcDelayCommand = 20 => 20x20ms = 0.4s = time to wait for a specific RC command to be acknowledged } else if ( (rcData[ROLL] > MAXCHECK) && rcData[PITCH] < MAXCHECK && !f.ARMED && calibratingG == 0 && f.ACC_CALIBRATED) { if (rcDelayCommand == 20) { f.ARMED = 1; headFreeModeHold = heading; } #endif #ifdef LCD_TELEMETRY_AUTO } else if (rcData[ROLL] < MINCHECK && rcData[PITCH] > MAXCHECK && !f.ARMED) { if (rcDelayCommand == 20) { if (telemetry_auto) { telemetry_auto = 0; telemetry = 0; } else telemetry_auto = 1; } #endif #ifdef LCD_TELEMETRY_STEP } else if (rcData[ROLL] > MAXCHECK && rcData[PITCH] > MAXCHECK && !f.ARMED) { if (rcDelayCommand == 20) { telemetry = telemetryStepSequence[++telemetryStepIndex % strlen(telemetryStepSequence)]; LCDclear(); // make sure to clear away remnants } #endif } else rcDelayCommand = 0; } else if (rcData[THROTTLE] > MAXCHECK && !f.ARMED) { if (rcData[YAW] < MINCHECK && rcData[PITCH] < MINCHECK) { // throttle=max, yaw=left, pitch=min if (rcDelayCommand == 20) calibratingA=400; rcDelayCommand++; } else if (rcData[YAW] > MAXCHECK && rcData[PITCH] < MINCHECK) { // throttle=max, yaw=right, pitch=min if (rcDelayCommand == 20) f.CALIBRATE_MAG = 1; // MAG calibration request rcDelayCommand++; } else if (rcData[PITCH] > MAXCHECK) { conf.angleTrim[PITCH]+=2;writeParams(1); #if defined(LED_RING) blinkLedRing(); #endif } else if (rcData[PITCH] < MINCHECK) { conf.angleTrim[PITCH]-=2;writeParams(1); #if defined(LED_RING) blinkLedRing(); #endif } else if (rcData[ROLL] > MAXCHECK) { conf.angleTrim[ROLL]+=2;writeParams(1); #if defined(LED_RING) blinkLedRing(); #endif } else if (rcData[ROLL] < MINCHECK) { conf.angleTrim[ROLL]-=2;writeParams(1); #if defined(LED_RING) blinkLedRing(); #endif } else { rcDelayCommand = 0; } } #if defined(LED_FLASHER) led_flasher_autoselect_sequence(); #endif #if defined(INFLIGHT_ACC_CALIBRATION) if (AccInflightCalibrationArmed && f.ARMED && rcData[THROTTLE] > MINCHECK && !rcOptions[BOXARM] ){ // Copter is airborne and you are turning it off via boxarm : start measurement InflightcalibratingA = 50; AccInflightCalibrationArmed = 0; } if (rcOptions[BOXPASSTHRU]) { // Use the Passthru Option to activate : Passthru = TRUE Meausrement started, Land and passtrhu = 0 measurement stored if (!AccInflightCalibrationActive && !AccInflightCalibrationMeasurementDone){ InflightcalibratingA = 50; } }else if(AccInflightCalibrationMeasurementDone && !f.ARMED){ AccInflightCalibrationMeasurementDone = 0; AccInflightCalibrationSavetoEEProm = 1; } #endif uint16_t auxState = 0; for(i=0;i<4;i++) auxState |= (rcData[AUX1+i]<1300)<<(3*i) | (1300<rcData[AUX1+i] && rcData[AUX1+i]<1700)<<(3*i+1) | (rcData[AUX1+i]>1700)<<(3*i+2); for(i=0;i<CHECKBOXITEMS;i++) rcOptions[i] = (auxState & conf.activate[i])>0; // note: if FAILSAFE is disable, failsafeCnt > 5*FAILSAVE_DELAY is always false if (( rcOptions[BOXACC] || (failsafeCnt > 5*FAILSAVE_DELAY) ) && ACC ) { // bumpless transfer to Level mode if (!f.ACC_MODE) { errorAngleI[ROLL] = 0; errorAngleI[PITCH] = 0; f.ACC_MODE = 1; } } else { // failsafe support f.ACC_MODE = 0; } if (rcOptions[BOXARM] == 0) f.OK_TO_ARM = 1; //if (f.ACC_MODE) {STABLEPIN_ON;} else {STABLEPIN_OFF;} #if BARO if (rcOptions[BOXBARO]) { if (!f.BARO_MODE) { f.BARO_MODE = 1; AltHold = EstAlt; initialThrottleHold = rcCommand[THROTTLE]; errorAltitudeI = 0; BaroPID=0; } } else { f.BARO_MODE = 0; } #endif #if MAG if (rcOptions[BOXMAG]) { if (!f.MAG_MODE) { f.MAG_MODE = 1; magHold = heading; } } else { f.MAG_MODE = 0; } if (rcOptions[BOXHEADFREE]) { if (!f.HEADFREE_MODE) { f.HEADFREE_MODE = 1; } } else { f.HEADFREE_MODE = 0; } if (rcOptions[BOXHEADADJ]) { headFreeModeHold = heading; // acquire new heading } #endif #if GPS #if defined(I2C_NAV) static uint8_t GPSNavReset = 1; if (f.GPS_FIX && GPS_numSat >= 5 ) { if (!rcOptions[BOXGPSHOME] && !rcOptions[BOXGPSHOLD] ) { //Both boxes are unselected if (GPSNavReset == 0 ) { GPSNavReset = 1; GPS_reset_nav(); } } if (rcOptions[BOXGPSHOME]) { if (!f.GPS_HOME_MODE) { f.GPS_HOME_MODE = 1; GPSNavReset = 0; GPS_I2C_command(I2C_GPS_COMMAND_START_NAV,0); //waypoint zero } } else { f.GPS_HOME_MODE = 0; } if (rcOptions[BOXGPSHOLD]) { if (!f.GPS_HOLD_MODE & !f.GPS_HOME_MODE) { f.GPS_HOLD_MODE = 1; GPSNavReset = 0; GPS_I2C_command(I2C_GPS_COMMAND_POSHOLD,0); } } else { f.GPS_HOLD_MODE = 0; } } #endif #if defined(GPS_SERIAL) || defined(TINY_GPS) || defined(GPS_FROM_OSD)||defined(I2C_GPS) if (f.GPS_FIX && GPS_numSat >= 5 ) { if (rcOptions[BOXGPSHOME]) { if (!f.GPS_HOME_MODE) { f.GPS_HOME_MODE = 1; GPS_set_next_wp(&GPS_home[LAT],&GPS_home[LON]); nav_mode = NAV_MODE_WP; } } else { f.GPS_HOME_MODE = 0; } if (rcOptions[BOXGPSHOLD]) { if (!f.GPS_HOLD_MODE) { f.GPS_HOLD_MODE = 1; GPS_hold[LAT] = GPS_coord[LAT]; GPS_hold[LON] = GPS_coord[LON]; GPS_set_next_wp(&GPS_hold[LAT],&GPS_hold[LON]); nav_mode = NAV_MODE_POSHOLD; } } else { f.GPS_HOLD_MODE = 0; } } #endif #endif #ifdef FLOW if(rcOptions[BOXFLOWHOLD]) { if (!f.FLOW_HOLD_MODE) { f.FLOW_HOLD_MODE = 1; GPS_hold[LAT] = GPS_coord[LAT]; GPS_hold[LON] = GPS_coord[LON]; flow_set_next_wp(&GPS_hold[LAT],&GPS_hold[LON]); nav_mode = NAV_MODE_POSHOLD; } //debug[1] = 1; } else { f.FLOW_HOLD_MODE = 0; //debug[1] = 0; } #endif if (rcOptions[BOXPASSTHRU]) {f.PASSTHRU_MODE = 1;} else {f.PASSTHRU_MODE = 0;} #ifdef FIXEDWING f.HEADFREE_MODE = 0; #endif } else { // not in rc loop static uint8_t taskOrder=0; // never call all functions in the same loop, to avoid high delay spikes if(taskOrder>4) taskOrder-=5; switch (taskOrder) { case 0: taskOrder++; #if MAG if (Mag_getADC()) break; // max 350 µs (HMC5883) // only break when we actually did something #endif case 1: taskOrder++; #if BARO if (Baro_update() != 0 ) break; #endif case 2: taskOrder++; #if BARO if (getEstimatedAltitude() !=0) break; #endif case 3: taskOrder++; if(flowUpdate() !=0) break; #if GPS if(GPS_Enable) GPS_NewData(); break; #endif case 4: taskOrder++; #if SONAR #if !defined(MAXSONAR_PWM) && !defined(FLOW) Sonar_update(); #endif //debug[2] = sonarAlt; #endif #ifdef LANDING_LIGHTS_DDR auto_switch_landing_lights(); #endif #ifdef VARIOMETER if (f.VARIO_MODE) vario_signaling(); #endif break; } } computeIMU(); // Measure loop rate just afer reading the sensors currentTime = micros(); cycleTime = currentTime - previousTime; previousTime = currentTime; //delayMs(500); //if(cycleTime > 3150) // debug[0]++; #if MAG if (fabs(rcCommand[YAW]) <70 && f.MAG_MODE) { int16_t dif = heading - magHold; if (dif <= - 180) dif += 360; if (dif >= + 180) dif -= 360; if ( f.SMALL_ANGLES_25 ) rcCommand[YAW] -= dif*conf.P8[PIDMAG]/30; // 18 deg } else magHold = heading; #endif #if BARO if (f.BARO_MODE) { if (fabs(rcCommand[THROTTLE]-initialThrottleHold)>ALT_HOLD_THROTTLE_NEUTRAL_ZONE) { f.BARO_MODE = 0; // so that a new althold reference is defined } rcCommand[THROTTLE] = initialThrottleHold + BaroPID; } #endif #if GPS if ( (!f.GPS_HOME_MODE && !f.GPS_HOLD_MODE) || !f.GPS_FIX_HOME ) { GPS_reset_nav(); // If GPS is not activated. Reset nav loops and all nav related parameters GPS_angle[ROLL] = 0; GPS_angle[PITCH] = 0; } else { float sin_yaw_y = sinf(heading*0.0174532925f); float cos_yaw_x = cosf(heading*0.0174532925f); #if defined(NAV_SLEW_RATE) nav_rated[LON] += constrain(wrap_18000(nav[LON]-nav_rated[LON]),-NAV_SLEW_RATE,NAV_SLEW_RATE); nav_rated[LAT] += constrain(wrap_18000(nav[LAT]-nav_rated[LAT]),-NAV_SLEW_RATE,NAV_SLEW_RATE); GPS_angle[ROLL] = (nav_rated[LON]*cos_yaw_x - nav_rated[LAT]*sin_yaw_y) /10; GPS_angle[PITCH] = (nav_rated[LON]*sin_yaw_y + nav_rated[LAT]*cos_yaw_x) /10; #else GPS_angle[ROLL] = (nav[LON]*cos_yaw_x - nav[LAT]*sin_yaw_y) /10; GPS_angle[PITCH] = (nav[LON]*sin_yaw_y + nav[LAT]*cos_yaw_x) /10; #endif } //debug[2] = GPS_angle[ROLL]; //debug[3] = GPS_angle[PITCH]; #endif #ifdef FLOW if (!f.FLOW_HOLD_MODE) { flow_reset_nav(); // If GPS is not activated. Reset nav loops and all nav related parameters GPS_angle[ROLL] = 0; GPS_angle[PITCH] = 0; } else { float sin_yaw_y = sinf(heading*0.0174532925f); float cos_yaw_x = cosf(heading*0.0174532925f); #if defined(NAV_SLEW_RATE) nav_rated[LON] += constrain(wrap_18000(nav[LON]-nav_rated[LON]),-NAV_SLEW_RATE,NAV_SLEW_RATE); nav_rated[LAT] += constrain(wrap_18000(nav[LAT]-nav_rated[LAT]),-NAV_SLEW_RATE,NAV_SLEW_RATE); GPS_angle[ROLL] = (nav_rated[LON]*cos_yaw_x - nav_rated[LAT]*sin_yaw_y) /10; GPS_angle[PITCH] = (nav_rated[LON]*sin_yaw_y + nav_rated[LAT]*cos_yaw_x) /10; #else GPS_angle[ROLL] = (nav[LON]*cos_yaw_x - nav[LAT]*sin_yaw_y) /10; GPS_angle[PITCH] = (nav[LON]*sin_yaw_y + nav[LAT]*cos_yaw_x) /10; #endif } //debug[2] = GPS_angle[ROLL]; //debug[3] = GPS_angle[PITCH]; #endif #if PIDcontroller == 1 //**** PITCH & ROLL & YAW PID **** for(axis=0;axis<3;axis++) { if (f.ACC_MODE && axis<2 ) { //LEVEL MODE // 50 degrees max inclination errorAngle = constrain(2*rcCommand[axis] + GPS_angle[axis],-500,+500) - angle[axis] + conf.angleTrim[axis]; //16 bits is ok here #ifdef LEVEL_PDF PTerm = -(int32_t)angle[axis]*conf.P8[PIDLEVEL]/100 ; #else PTerm = (int32_t)errorAngle*conf.P8[PIDLEVEL]/100 ; // 32 bits is needed for calculation: errorAngle*P8[PIDLEVEL] could exceed 32768 16 bits is ok for result #endif PTerm = constrain(PTerm,-conf.D8[PIDLEVEL]*5,+conf.D8[PIDLEVEL]*5); errorAngleI[axis] = constrain(errorAngleI[axis]+errorAngle,-10000,+10000); // WindUp //16 bits is ok here ITerm = ((int32_t)errorAngleI[axis]*conf.I8[PIDLEVEL])>>12; // 32 bits is needed for calculation:10000*I8 could exceed 32768 16 bits is ok for result } else { //ACRO MODE or YAW axis if (fabs(rcCommand[axis])<350) error = rcCommand[axis]*10*8/conf.P8[axis] ; // 16 bits is needed for calculation: 350*10*8 = 28000 16 bits is ok for result if P8>2 (P>0.2) else error = (int32_t)rcCommand[axis]*10*8/conf.P8[axis] ; // 32 bits is needed for calculation: 500*5*10*8 = 200000 16 bits is ok for result if P8>2 (P>0.2) error -= gyroData[axis]; PTerm = rcCommand[axis]; errorGyroI[axis] = constrain(errorGyroI[axis]+error,-16000,+16000); // WindUp 16 bits is ok here if (fabs(gyroData[axis])>640) errorGyroI[axis] = 0; ITerm = (errorGyroI[axis]/125*conf.I8[axis])>>6; // 16 bits is ok here 16000/125 = 128 ; 128*250 = 32000 } if (fabs(gyroData[axis])<160) PTerm -= gyroData[axis]*dynP8[axis]/10/8; // 16 bits is needed for calculation 160*200 = 32000 16 bits is ok for result else PTerm -= (int32_t)gyroData[axis]*dynP8[axis]/10/8; // 32 bits is needed for calculation delta = gyroData[axis] - lastGyro[axis]; // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800 lastGyro[axis] = gyroData[axis]; deltaSum = delta1[axis]+delta2[axis]+delta; delta2[axis] = delta1[axis]; delta1[axis] = delta; if (fabs(deltaSum)<640) DTerm = (deltaSum*dynD8[axis])>>5; // 16 bits is needed for calculation 640*50 = 32000 16 bits is ok for result else DTerm = ((int32_t)deltaSum*dynD8[axis])>>5; // 32 bits is needed for calculation
uint8_t calculateBatteryCapacityRemainingPercentage(void) { uint16_t batteryCapacity = batteryConfig()->batteryCapacity; return constrain((batteryCapacity - constrain(mAhDrawn, 0, 0xFFFF)) * 100.0f / batteryCapacity , 0, 100); }
static void evaluateCommand(void) { uint32_t i, j, tmp, junk; #ifdef GPS uint8_t wp_no; int32_t lat = 0, lon = 0, alt = 0; #endif if (osdSilence) osdDisableMSP(); switch (currentPortState->cmdMSP) { case MSP_SET_RAW_RC: for (i = 0; i < 8; i++) rcData[i] = read16(); headSerialReply(0); mspFrameRecieve(); break; case MSP_SET_ACC_TRIM: cfg.angleTrim[PITCH] = read16(); cfg.angleTrim[ROLL] = read16(); headSerialReply(0); break; #ifdef GPS case MSP_SET_RAW_GPS: f.GPS_FIX = read8(); GPS_numSat = read8(); GPS_coord[LAT] = read32(); GPS_coord[LON] = read32(); GPS_altitude = read16(); GPS_speed = read16(); GPS_update |= 2; // New data signalisation to GPS functions headSerialReply(0); break; #endif case MSP_SET_PID: for (i = 0; i < PIDITEMS; i++) { cfg.P8[i] = read8(); cfg.I8[i] = read8(); cfg.D8[i] = read8(); } headSerialReply(0); break; case MSP_SET_BOX: for (i = 0; i < numberBoxItems; i++) cfg.activate[availableBoxes[i]] = read16(); headSerialReply(0); break; case MSP_SET_RC_TUNING: cfg.rcRate8 = read8(); cfg.rcExpo8 = read8(); cfg.rollPitchRate = read8(); cfg.yawRate = read8(); cfg.dynThrPID = read8(); cfg.thrMid8 = read8(); cfg.thrExpo8 = read8(); headSerialReply(0); break; case MSP_SET_MISC: tmp = read16(); // sanity check if (tmp < 1600 && tmp > 1400) mcfg.midrc = tmp; mcfg.minthrottle = read16(); mcfg.maxthrottle = read16(); mcfg.mincommand = read16(); cfg.failsafe_throttle = read16(); mcfg.gps_type = read8(); mcfg.gps_baudrate = read8(); mcfg.gps_ubx_sbas = read8(); mcfg.multiwiicurrentoutput = read8(); read16(); cfg.mag_declination = read16() * 10; mcfg.vbatscale = read8(); // actual vbatscale as intended mcfg.vbatmincellvoltage = read8(); // vbatlevel_warn1 in MWC2.3 GUI mcfg.vbatmaxcellvoltage = read8(); // vbatlevel_warn2 in MWC2.3 GUI read8(); // vbatlevel_crit (unused) headSerialReply(0); break; case MSP_SET_MOTOR: for (i = 0; i < 8; i++) motor_disarmed[i] = read16(); headSerialReply(0); break; case MSP_SELECT_SETTING: if (!f.ARMED) { mcfg.current_profile = read8(); if (mcfg.current_profile > 2) mcfg.current_profile = 0; // this writes new profile index and re-reads it writeEEPROM(0, false); } headSerialReply(0); break; case MSP_SET_HEAD: magHold = read16(); headSerialReply(0); break; case MSP_IDENT: headSerialReply(7); serialize8(VERSION); // multiwii version serialize8(mcfg.mixerConfiguration); // type of multicopter serialize8(MSP_VERSION); // MultiWii Serial Protocol Version serialize32(CAP_PLATFORM_32BIT | CAP_BASEFLIGHT_CONFIG | CAP_DYNBALANCE | (mcfg.flaps_speed ? CAP_FLAPS : 0)); // "capability" break; case MSP_STATUS: headSerialReply(11); serialize16(cycleTime); serialize16(i2cGetErrorCounter()); serialize16(sensors(SENSOR_ACC) | sensors(SENSOR_BARO) << 1 | sensors(SENSOR_MAG) << 2 | sensors(SENSOR_GPS) << 3 | sensors(SENSOR_SONAR) << 4); // OK, so you waste all the f*****g time to have BOXNAMES and BOXINDEXES etc, and then you go ahead and serialize enabled shit simply by stuffing all // the bits in order, instead of setting the enabled bits based on BOXINDEX. WHERE IS THE F*****G LOGIC IN THIS, FUCKWADS. // Serialize the boxes in the order we delivered them, until multiwii retards fix their shit junk = 0; tmp = f.ANGLE_MODE << BOXANGLE | f.HORIZON_MODE << BOXHORIZON | f.BARO_MODE << BOXBARO | f.MAG_MODE << BOXMAG | f.HEADFREE_MODE << BOXHEADFREE | rcOptions[BOXHEADADJ] << BOXHEADADJ | rcOptions[BOXCAMSTAB] << BOXCAMSTAB | rcOptions[BOXCAMTRIG] << BOXCAMTRIG | f.GPS_HOME_MODE << BOXGPSHOME | f.GPS_HOLD_MODE << BOXGPSHOLD | f.PASSTHRU_MODE << BOXPASSTHRU | rcOptions[BOXBEEPERON] << BOXBEEPERON | rcOptions[BOXLEDMAX] << BOXLEDMAX | rcOptions[BOXLLIGHTS] << BOXLLIGHTS | rcOptions[BOXVARIO] << BOXVARIO | rcOptions[BOXCALIB] << BOXCALIB | rcOptions[BOXGOV] << BOXGOV | rcOptions[BOXOSD] << BOXOSD | rcOptions[BOXTELEMETRY] << BOXTELEMETRY | f.ARMED << BOXARM; for (i = 0; i < numberBoxItems; i++) { int flag = (tmp & (1 << availableBoxes[i])); if (flag) junk |= 1 << i; } serialize32(junk); serialize8(mcfg.current_profile); break; case MSP_RAW_IMU: headSerialReply(18); // Retarded hack until multiwiidorks start using real units for sensor data if (acc_1G > 1024) { for (i = 0; i < 3; i++) serialize16(accSmooth[i] / 8); } else { for (i = 0; i < 3; i++) serialize16(accSmooth[i]); } for (i = 0; i < 3; i++) serialize16(gyroData[i]); for (i = 0; i < 3; i++) serialize16(magADC[i]); break; case MSP_SERVO: s_struct((uint8_t *)&servo, 16); break; case MSP_SERVO_CONF: headSerialReply(56); for (i = 0; i < MAX_SERVOS; i++) { serialize16(cfg.servoConf[i].min); serialize16(cfg.servoConf[i].max); serialize16(cfg.servoConf[i].middle); serialize8(cfg.servoConf[i].rate); } break; case MSP_SET_SERVO_CONF: headSerialReply(0); for (i = 0; i < MAX_SERVOS; i++) { cfg.servoConf[i].min = read16(); cfg.servoConf[i].max = read16(); cfg.servoConf[i].middle = read16(); cfg.servoConf[i].rate = read8(); } break; case MSP_MOTOR: s_struct((uint8_t *)motor, 16); break; case MSP_RC: headSerialReply(16); for (i = 0; i < 8; i++) serialize16(rcData[i]); break; #ifdef GPS case MSP_RAW_GPS: headSerialReply(16); serialize8(f.GPS_FIX); serialize8(GPS_numSat); serialize32(GPS_coord[LAT]); serialize32(GPS_coord[LON]); serialize16(GPS_altitude); serialize16(GPS_speed); serialize16(GPS_ground_course); break; case MSP_COMP_GPS: headSerialReply(5); serialize16(GPS_distanceToHome); serialize16(GPS_directionToHome); serialize8(GPS_update & 1); break; #endif case MSP_ATTITUDE: headSerialReply(6); for (i = 0; i < 2; i++) serialize16(angle[i]); serialize16(heading); break; case MSP_ALTITUDE: headSerialReply(6); serialize32(EstAlt); serialize16(vario); break; case MSP_ANALOG: headSerialReply(7); serialize8((uint8_t)constrain(vbat, 0, 255)); serialize16((uint16_t)constrain(mAhdrawn, 0, 0xFFFF)); // milliamphours drawn from battery serialize16(rssi); if (mcfg.multiwiicurrentoutput) serialize16((uint16_t)constrain((abs(amperage) * 10), 0, 0xFFFF)); // send amperage in 0.001 A steps else serialize16((uint16_t)constrain(abs(amperage), 0, 0xFFFF)); // send amperage in 0.01 A steps break; case MSP_RC_TUNING: headSerialReply(7); serialize8(cfg.rcRate8); serialize8(cfg.rcExpo8); serialize8(cfg.rollPitchRate); serialize8(cfg.yawRate); serialize8(cfg.dynThrPID); serialize8(cfg.thrMid8); serialize8(cfg.thrExpo8); break; case MSP_PID: headSerialReply(3 * PIDITEMS); for (i = 0; i < PIDITEMS; i++) { serialize8(cfg.P8[i]); serialize8(cfg.I8[i]); serialize8(cfg.D8[i]); } break; case MSP_PIDNAMES: headSerialReply(sizeof(pidnames) - 1); serializeNames(pidnames); break; case MSP_BOX: headSerialReply(2 * numberBoxItems); for (i = 0; i < numberBoxItems; i++) serialize16(cfg.activate[availableBoxes[i]]); break; case MSP_BOXNAMES: // headSerialReply(sizeof(boxnames) - 1); serializeBoxNamesReply(); break; case MSP_BOXIDS: headSerialReply(numberBoxItems); for (i = 0; i < numberBoxItems; i++) { for (j = 0; j < CHECKBOXITEMS; j++) { if (boxes[j].permanentId == availableBoxes[i]) serialize8(boxes[j].permanentId); } } break; case MSP_MISC: headSerialReply(2 * 6 + 4 + 2 + 4); serialize16(mcfg.midrc); serialize16(mcfg.minthrottle); serialize16(mcfg.maxthrottle); serialize16(mcfg.mincommand); serialize16(cfg.failsafe_throttle); serialize8(mcfg.gps_type); serialize8(mcfg.gps_baudrate); serialize8(mcfg.gps_ubx_sbas); serialize8(mcfg.multiwiicurrentoutput); serialize16(0); serialize16(cfg.mag_declination / 10); // TODO check this shit serialize8(mcfg.vbatscale); serialize8(mcfg.vbatmincellvoltage); serialize8(mcfg.vbatmaxcellvoltage); serialize8(0); break; case MSP_MOTOR_PINS: headSerialReply(8); for (i = 0; i < 8; i++) serialize8(i + 1); break; #ifdef GPS case MSP_WP: wp_no = read8(); // get the wp number headSerialReply(18); if (wp_no == 0) { lat = GPS_home[LAT]; lon = GPS_home[LON]; } else if (wp_no == 16) { lat = GPS_hold[LAT]; lon = GPS_hold[LON]; } serialize8(wp_no); serialize32(lat); serialize32(lon); serialize32(AltHold); // altitude (cm) will come here -- temporary implementation to test feature with apps serialize16(0); // heading will come here (deg) serialize16(0); // time to stay (ms) will come here serialize8(0); // nav flag will come here break; case MSP_SET_WP: wp_no = read8(); //get the wp number lat = read32(); lon = read32(); alt = read32(); // to set altitude (cm) read16(); // future: to set heading (deg) read16(); // future: to set time to stay (ms) read8(); // future: to set nav flag if (wp_no == 0) { GPS_home[LAT] = lat; GPS_home[LON] = lon; f.GPS_HOME_MODE = 0; // with this flag, GPS_set_next_wp will be called in the next loop -- OK with SERIAL GPS / OK with I2C GPS f.GPS_FIX_HOME = 1; if (alt != 0) AltHold = alt; // temporary implementation to test feature with apps } else if (wp_no == 16) { // OK with SERIAL GPS -- NOK for I2C GPS / needs more code dev in order to inject GPS coord inside I2C GPS GPS_hold[LAT] = lat; GPS_hold[LON] = lon; if (alt != 0) AltHold = alt; // temporary implementation to test feature with apps nav_mode = NAV_MODE_WP; GPS_set_next_wp(&GPS_hold[LAT], &GPS_hold[LON]); } headSerialReply(0); break; #endif /* GPS */ case MSP_RESET_CONF: if (!f.ARMED) checkFirstTime(true); headSerialReply(0); break; case MSP_ACC_CALIBRATION: if (!f.ARMED) calibratingA = CALIBRATING_ACC_CYCLES; headSerialReply(0); break; case MSP_MAG_CALIBRATION: if (!f.ARMED) f.CALIBRATE_MAG = 1; headSerialReply(0); break; case MSP_EEPROM_WRITE: if (f.ARMED) { headSerialError(0); } else { writeEEPROM(0, true); headSerialReply(0); } break; case MSP_DEBUG: headSerialReply(8); // make use of this crap, output some useful QA statistics debug[3] = ((hse_value / 1000000) * 1000) + (SystemCoreClock / 1000000); // XX0YY [crystal clock : core clock] for (i = 0; i < 4; i++) serialize16(debug[i]); // 4 variables are here for general monitoring purpose break; // Additional commands that are not compatible with MultiWii case MSP_ACC_TRIM: headSerialReply(4); serialize16(cfg.angleTrim[PITCH]); serialize16(cfg.angleTrim[ROLL]); break; case MSP_UID: headSerialReply(12); serialize32(U_ID_0); serialize32(U_ID_1); serialize32(U_ID_2); break; case MSP_GPSSVINFO: headSerialReply(1 + (GPS_numCh * 4)); serialize8(GPS_numCh); for (i = 0; i < GPS_numCh; i++){ serialize8(GPS_svinfo_chn[i]); serialize8(GPS_svinfo_svid[i]); serialize8(GPS_svinfo_quality[i]); serialize8(GPS_svinfo_cno[i]); } break; case MSP_SET_CONFIG: headSerialReply(0); mcfg.mixerConfiguration = read8(); // multitype featureClearAll(); featureSet(read32()); // features bitmap mcfg.serialrx_type = read8(); // serialrx_type mcfg.board_align_roll = read16(); // board_align_roll mcfg.board_align_pitch = read16(); // board_align_pitch mcfg.board_align_yaw = read16(); // board_align_yaw mcfg.currentscale = read16(); mcfg.currentoffset = read16(); /// ??? break; case MSP_CONFIG: headSerialReply(1 + 4 + 1 + 2 + 2 + 2 + 4); serialize8(mcfg.mixerConfiguration); serialize32(featureMask()); serialize8(mcfg.serialrx_type); serialize16(mcfg.board_align_roll); serialize16(mcfg.board_align_pitch); serialize16(mcfg.board_align_yaw); serialize16(mcfg.currentscale); serialize16(mcfg.currentoffset); /// ??? break; case MSP_RCMAP: headSerialReply(MAX_INPUTS); // TODO fix this for (i = 0; i < MAX_INPUTS; i++) serialize8(mcfg.rcmap[i]); break; case MSP_SET_RCMAP: headSerialReply(0); for (i = 0; i < MAX_INPUTS; i++) mcfg.rcmap[i] = read8(); break; case MSP_REBOOT: headSerialReply(0); pendReboot = true; break; case MSP_SILENCE_OSD: headSerialReply(0); osdSilence = true; break; default: // we do not know how to handle the (valid) message, indicate error MSP $M! headSerialError(0); break; } tailSerialReply(); }
double simplex(double (*objfunc)(double[], void *extra), double start[],int n, double EPSILON, double scale, void (*constrain)(double[],int n), void *extra) { int vs; /* vertex with smallest value */ int vh; /* vertex with next smallest value */ int vg; /* vertex with largest value */ int i,j,m,row; int k; /* track the number of function evaluations */ int itr; /* track the number of iterations */ double **v; /* holds vertices of simplex */ double pn,qn; /* values used to create initial simplex */ double *f; /* value of function at each vertex */ double fr; /* value of function at reflection point */ double fe; /* value of function at expansion point */ double fc; /* value of function at contraction point */ double *vr; /* reflection - coordinates */ double *ve; /* expansion - coordinates */ double *vc; /* contraction - coordinates */ double *vm; /* centroid - coordinates */ double min; double fsum,favg,s,cent; /* dynamically allocate arrays */ /* allocate the rows of the arrays */ v = (double **) malloc ((n+1) * sizeof(double *)); f = (double *) malloc ((n+1) * sizeof(double)); vr = (double *) malloc (n * sizeof(double)); ve = (double *) malloc (n * sizeof(double)); vc = (double *) malloc (n * sizeof(double)); vm = (double *) malloc (n * sizeof(double)); /* allocate the columns of the arrays */ for (i=0;i<=n;i++) { v[i] = (double *) malloc (n * sizeof(double)); } /* create the initial simplex */ /* assume one of the vertices is 0,0 */ pn = scale*(sqrt(n+1)-1+n)/(n*sqrt(2)); qn = scale*(sqrt(n+1)-1)/(n*sqrt(2)); for (i=0;i<n;i++) { v[0][i] = start[i]; } for (i=1;i<=n;i++) { for (j=0;j<n;j++) { if (i-1 == j) { v[i][j] = pn + start[j]; } else { v[i][j] = qn + start[j]; } } } if (constrain != NULL) { constrain(v[j],n); } /* find the initial function values */ for (j=0;j<=n;j++) { f[j] = objfunc(v[j], extra); } k = n+1; /* print out the initial values */ /* printf("Initial Values\n"); for (j=0;j<=n;j++) { for (i=0;i<n;i++) { printf("%f %f\n",v[j][i],f[j]); } } */ /* begin the main loop of the minimization */ for (itr=1;itr<=MAX_IT;itr++) { /* find the index of the largest value */ vg=0; for (j=0;j<=n;j++) { if (f[j] > f[vg]) { vg = j; } } /* find the index of the smallest value */ vs=0; for (j=0;j<=n;j++) { if (f[j] < f[vs]) { vs = j; } } /* find the index of the second largest value */ vh=vs; for (j=0;j<=n;j++) { if (f[j] > f[vh] && f[j] < f[vg]) { vh = j; } } /* calculate the centroid */ for (j=0;j<=n-1;j++) { cent=0.0; for (m=0;m<=n;m++) { if (m!=vg) { cent += v[m][j]; } } vm[j] = cent/n; } /* reflect vg to new vertex vr */ for (j=0;j<=n-1;j++) { /*vr[j] = (1+ALPHA)*vm[j] - ALPHA*v[vg][j];*/ vr[j] = vm[j]+ALPHA*(vm[j]-v[vg][j]); } if (constrain != NULL) { constrain(vr,n); } fr = objfunc(vr, extra); k++; if (fr < f[vh] && fr >= f[vs]) { for (j=0;j<=n-1;j++) { v[vg][j] = vr[j]; } f[vg] = fr; } /* investigate a step further in this direction */ if ( fr < f[vs]) { for (j=0;j<=n-1;j++) { /*ve[j] = GAMMA*vr[j] + (1-GAMMA)*vm[j];*/ ve[j] = vm[j]+GAMMA*(vr[j]-vm[j]); } if (constrain != NULL) { constrain(ve,n); } fe = objfunc(ve, extra); k++; /* by making fe < fr as opposed to fe < f[vs], Rosenbrocks function takes 63 iterations as opposed to 64 when using double variables. */ if (fe < fr) { for (j=0;j<=n-1;j++) { v[vg][j] = ve[j]; } f[vg] = fe; } else { for (j=0;j<=n-1;j++) { v[vg][j] = vr[j]; } f[vg] = fr; } } /* check to see if a contraction is necessary */ if (fr >= f[vh]) { if (fr < f[vg] && fr >= f[vh]) { /* perform outside contraction */ for (j=0;j<=n-1;j++) { /*vc[j] = BETA*v[vg][j] + (1-BETA)*vm[j];*/ vc[j] = vm[j]+BETA*(vr[j]-vm[j]); } if (constrain != NULL) { constrain(vc,n); } fc = objfunc(vc, extra); k++; } else { /* perform inside contraction */ for (j=0;j<=n-1;j++) { /*vc[j] = BETA*v[vg][j] + (1-BETA)*vm[j];*/ vc[j] = vm[j]-BETA*(vm[j]-v[vg][j]); } if (constrain != NULL) { constrain(vc,n); } fc = objfunc(vc, extra); k++; } if (fc < f[vg]) { for (j=0;j<=n-1;j++) { v[vg][j] = vc[j]; } f[vg] = fc; } /* at this point the contraction is not successful, we must halve the distance from vs to all the vertices of the simplex and then continue. 10/31/97 - modified to account for ALL vertices. */ else { for (row=0;row<=n;row++) { if (row != vs) { for (j=0;j<=n-1;j++) { v[row][j] = v[vs][j]+(v[row][j]-v[vs][j])/2.0; } } } if (constrain != NULL) { constrain(v[vg],n); } f[vg] = objfunc(v[vg], extra); k++; if (constrain != NULL) { constrain(v[vh],n); } f[vh] = objfunc(v[vh], extra); k++; } } /* print out the value at each iteration */ /* printf("Iteration %d\n",itr); for (j=0;j<=n;j++) { for (i=0;i<n;i++) { printf("%f %f\n",v[j][i],f[j]); } } */ /* test for convergence */ fsum = 0.0; for (j=0;j<=n;j++) { fsum += f[j]; } favg = fsum/(n+1); s = 0.0; for (j=0;j<=n;j++) { s += pow((f[j]-favg),2.0)/(n); } s = sqrt(s); if (s < EPSILON) break; } /* end main loop of the minimization */ /* find the index of the smallest value */ vs=0; for (j=0;j<=n;j++) { if (f[j] < f[vs]) { vs = j; } } // printf("The minimum was found at\n"); for (j=0;j<n;j++) { // printf("%e\n",v[vs][j]); start[j] = v[vs][j]; } min=objfunc(v[vs], extra); k++; // printf("%d Function Evaluations\n",k); // printf("%d Iterations through program\n",itr); free(f); free(vr); free(ve); free(vc); free(vm); for (i=0;i<=n;i++) { free (v[i]); } free(v); return min; }
void processRcAdjustments(controlRateConfig_t *controlRateConfig) { const uint32_t now = millis(); int newValue = -1; const bool canUseRxData = rxIsReceivingSignal(); for (int adjustmentIndex = 0; adjustmentIndex < MAX_SIMULTANEOUS_ADJUSTMENT_COUNT; adjustmentIndex++) { adjustmentState_t *adjustmentState = &adjustmentStates[adjustmentIndex]; if (!adjustmentState->config) { continue; } const uint8_t adjustmentFunction = adjustmentState->config->adjustmentFunction; if (adjustmentFunction == ADJUSTMENT_NONE) { continue; } if (cmp32(now, adjustmentState->timeoutAt) >= 0) { adjustmentState->timeoutAt = now + RESET_FREQUENCY_2HZ; MARK_ADJUSTMENT_FUNCTION_AS_READY(adjustmentIndex); #if defined(USE_OSD) && defined(USE_OSD_ADJUSTMENTS) adjustmentRangeValue = -1; #endif } if (!canUseRxData) { continue; } const uint8_t channelIndex = NON_AUX_CHANNEL_COUNT + adjustmentState->auxChannelIndex; if (adjustmentState->config->mode == ADJUSTMENT_MODE_STEP) { int delta; if (rcData[channelIndex] > rxConfig()->midrc + 200) { delta = adjustmentState->config->data.step; } else if (rcData[channelIndex] < rxConfig()->midrc - 200) { delta = -adjustmentState->config->data.step; } else { // returning the switch to the middle immediately resets the ready state MARK_ADJUSTMENT_FUNCTION_AS_READY(adjustmentIndex); adjustmentState->timeoutAt = now + RESET_FREQUENCY_2HZ; continue; } if (IS_ADJUSTMENT_FUNCTION_BUSY(adjustmentIndex)) { continue; } newValue = applyStepAdjustment(controlRateConfig, adjustmentFunction, delta); pidInitConfig(pidProfile); } else if (adjustmentState->config->mode == ADJUSTMENT_MODE_SELECT) { int switchPositions = adjustmentState->config->data.switchPositions; if (adjustmentFunction == ADJUSTMENT_RATE_PROFILE && systemConfig()->rateProfile6PosSwitch) { switchPositions = 6; } const uint16_t rangeWidth = (2100 - 900) / switchPositions; const uint8_t position = (constrain(rcData[channelIndex], 900, 2100 - 1) - 900) / rangeWidth; newValue = applySelectAdjustment(adjustmentFunction, position); } #if defined(USE_OSD) && defined(USE_OSD_ADJUSTMENTS) if (newValue != -1 && adjustmentState->config->adjustmentFunction != ADJUSTMENT_RATE_PROFILE) { // Rate profile already has an OSD element adjustmentRangeName = &adjustmentLabels[adjustmentFunction - 1][0]; adjustmentRangeValue = newValue; } #else UNUSED(newValue); #endif MARK_ADJUSTMENT_FUNCTION_AS_BUSY(adjustmentIndex); } }
void RemoteControl::editar_pos (byte comando) { #define EXTREMOS 0 #define MEDIOS 1 static byte edit_pos_modo = EXTREMOS; switch (comando) { case RC_UP: texto1 = "|"; if (edit_pos_modo==EXTREMOS) { edit_position (mov.get_pos_ref(), 0, 0, step, 0); mov.goto_pos_ref(); } break; case RC_DOWN: texto1 = "^"; if (edit_pos_modo==EXTREMOS) { edit_position (mov.get_pos_ref(), 0, 0, -step, 0); mov.goto_pos_ref(); } break; case RC_RIGHT: texto1 = "<"; if (edit_pos_modo==EXTREMOS) { edit_position (mov.get_pos_ref(), 0, step, 0, 0); } else { edit_position (mov.get_pos_ref(), 0, 0, 0, step); } mov.goto_pos_ref(); break; case RC_LEFT: texto1 = ">"; if (edit_pos_modo==EXTREMOS) { edit_position (mov.get_pos_ref(), 0, -step, 0, 0); } else { edit_position (mov.get_pos_ref(), 0, 0, 0, -step); } mov.goto_pos_ref(); break; case RC_MENU: texto1 = "EXTREMOS"; edit_pos_modo = EXTREMOS; break; case RC_MTS: texto1 = "MEDIOS"; edit_pos_modo = MEDIOS; break; case RC_EXIT: texto1 = "h-"; edit_position (mov.get_pos_ref(), step, 0, 0, 0); mov.translation ((COORD3D) {0, step, 0}, step/(velocidad*TICK)); // sustituir la fórmula de velocidad por un número concreto break; case RC_CCTTX: texto1 = "h+"; edit_position (mov.get_pos_ref(), -step, 0, 0, 0); mov.translation ((COORD3D) {0, -step, 0}, step/(velocidad*TICK)); break; // esto también se repite case RC_CH_UP: if (pantalla.isBusy()) {break;} if (!isMoving) {step = constrain (step+inc, .5, 5);} texto1 = "Step " + float2string (step); retardo = true; break; case RC_CH_DN: if (pantalla.isBusy()) {break;} if (!isMoving) {step = constrain (step-inc, .5, 5);} texto1 = "Step " + float2string (step); retardo = true; break; } }
static float get_I(float error, float *dt, struct PID_ *pid, struct PID_PARAM_ *pid_param) { pid->integrator += (error * pid_param->kI * *dt); pid->integrator = constrain(pid->integrator, -pid_param->Imax, pid_param->Imax); return pid->integrator; }
void RemoteControl::procesar_comando (byte comando) { // acá se procesan las teclas generales, o que tienen una función única // las otras son procesadas por la rutina específica de cada modo texto1 = "def"; switch (comando) { case RC_POWER: if (isMoving || pantalla.isBusy()) {break;} if (modo == REMOTE_OFF) { color1 = RGB(0, 2, 3); texto1 = "REMOTE ON"; modo = CAMINATAS1; } else { color1 = RGB(2, 3, 2); texto1 = "REMOTE OFF"; modo = REMOTE_OFF; } break; case RC_SUSPEND: // una de estas tiene que resetear incluso la velocidad, paso, etc. (para facilitar la edicion) if (isMoving || pantalla.isBusy()) {break;} angulo_offset = 0; centro_caminata = (COORD2D) {0, 0}; texto1 = "suspend"; mov.goto_pos_ref (DEFAULT_POSITION); break; case RC_SLEEP: break; case RC_HOME: if (isMoving || pantalla.isBusy() || modo==REMOTE_OFF) {break;} angulo_offset = 0; centro_caminata = (COORD2D) {0, 0}; texto1 = "home"; mov.goto_pos_ref (); break; case RC_DVD1: pagina = constrain (pagina+1, 0, 10); // este '10' hay que hacer un #define MAX_PAGINA texto1 = "Pagina " + String (pagina, DEC); break; case RC_FM: pagina = constrain (pagina-1, 0, 10); // este '10' hay que hacer un #define MAX_PAGINA texto1 = "Pagina " + String (pagina, DEC); break; // teclas numéricas //case 0: case 1: case 2: case 3: case 4: case 5: case 6: case 7: case 8: case 9: break; case RC_CH_RTN: break; case RC_ENTER2: break; case RC_MUTE: break; case RC_REW: break; case RC_PLAYPAUSE: break; case RC_FFWD: break; case RC_REC: break; case RC_PREV: break; case RC_STOP: break; case RC_NEXT: break; default: // botones de modo if (comando>=RC_TV && comando<=RC_APPLICATION) { switch (comando + 100*pagina) { case RC_TV: if (isMoving || pantalla.isBusy() || modo==REMOTE_OFF) {break;} color1 = RGB(0, 2, 3); texto1 = "caminatas 1"; modo = CAMINATAS1; break; case RC_FMRADIO: if (isMoving || pantalla.isBusy() || modo==REMOTE_OFF) {break;} color1 = RGB(1, 3, 1); texto1 = "caminatas 2"; modo = CAMINATAS2; break; case RC_MUSIC: if (isMoving || pantalla.isBusy() || modo==REMOTE_OFF) {break;} color1 = RGB(0, 3, 1); texto1 = "traslaciones"; modo = TRASLACIONES; break; case RC_PICTURES: if (isMoving || pantalla.isBusy() || modo==REMOTE_OFF) {break;} color1 = RGB(2, 2, 0); texto1 = "rotaciones"; modo = ROTACIONES; break; case RC_VIDEOCLIP: if (isMoving || pantalla.isBusy() || modo==REMOTE_OFF) {break;} color1 = RGB(2, 1, 3); texto1 = "editar centro"; modo = EDITAR_CENTRO; break; case RC_DVD2: if (isMoving || pantalla.isBusy() || modo==REMOTE_OFF) {break;} color1 = RGB(1, 3, 3); texto1 = "editar posicion"; modo = EDITAR_POS; mov.goto_pos_ref (); break; case RC_GAMEZONE: if (isMoving || pantalla.isBusy() || modo==REMOTE_OFF) {break;} color1 = RGB(2, 1, 2); texto1 = "osc. traslacion"; modo = OSCILADORES1; break; case RC_APPLICATION: if (isMoving || pantalla.isBusy() || modo==REMOTE_OFF) {break;} color1 = RGB(1, 0, 3); texto1 = "osc. rotacion"; modo = OSCILADORES3; break; case RC_TV+100: if (isMoving || pantalla.isBusy() || modo==REMOTE_OFF) {break;} color1 = RGB(2, 2, 0); texto1 = "osc. centro"; modo = OSCILADORES2; break; case RC_FMRADIO+100: if (isMoving || pantalla.isBusy() || modo==REMOTE_OFF) {break;} color1 = RGB(1, 2, 1); texto1 = "conductas"; modo = CONDUCTAS; break; } } else { switch (modo) { case CAMINATAS1: case CAMINATAS2: caminatas (comando); break; case TRASLACIONES: traslaciones (comando); break; case ROTACIONES: rotaciones (comando); break; case EDITAR_CENTRO: editar_centro (comando); break; case EDITAR_POS: editar_pos (comando); break; case OSCILADORES1: case OSCILADORES2: case OSCILADORES3: osciladores (comando); break; case CONDUCTAS: conductas (comando); break; } } } displayText (); }
inline const _Tp expo(const _Tp &value, const _Tp &e) { _Tp x = constrain(value, (_Tp) - 1, (_Tp)1); return (1 - e) * x + e * x * x * x; }
void LinearActuator::normalizePosition() { _position = constrain(_position, LA_RAW_POS_MIN, LA_RAW_POS_MAX); _position = map(_position, LA_RAW_POS_MIN, LA_RAW_POS_MAX, 1023, 0); }
inline const _Tp superexpo(const _Tp &value, const _Tp &e, const _Tp &g) { _Tp x = constrain(value, (_Tp) - 1, (_Tp)1); return expo(x, e) * (1 - g) / (1 - fabsf(x) * g); }
void PID_autotune(float temp, int hotend, int ncycles) { float input = 0.0; int cycles = 0; bool heating = true; millis_t temp_ms = millis(), t1 = temp_ms, t2 = temp_ms; long t_high = 0, t_low = 0; long bias, d; float Ku, Tu; float Kp_temp, Ki_temp, Kd_temp; float max = 0, min = 10000; #if HAS_AUTO_FAN millis_t next_auto_fan_check_ms = temp_ms + 2500; #endif if (hotend >= HOTENDS #if !HAS_TEMP_BED || hotend < 0 #endif ) { ECHO_LM(ER, MSG_PID_BAD_EXTRUDER_NUM); return; } ECHO_LM(DB, MSG_PID_AUTOTUNE_START); if (hotend < 0) { ECHO_SM(DB, "BED"); } else { ECHO_SMV(DB, "Hotend: ", hotend); } ECHO_MV(" Temp: ", temp); ECHO_EMV(" Cycles: ", ncycles); disable_all_heaters(); // switch off all heaters. if (hotend < 0) soft_pwm_bed = bias = d = MAX_BED_POWER / 2; else soft_pwm[hotend] = bias = d = PID_MAX / 2; // PID Tuning loop for (;;) { millis_t ms = millis(); if (temp_meas_ready) { // temp sample ready updateTemperaturesFromRawValues(); input = (hotend<0)?current_temperature_bed:current_temperature[hotend]; max = max(max, input); min = min(min, input); #if HAS_AUTO_FAN if (ms > next_auto_fan_check_ms) { checkExtruderAutoFans(); next_auto_fan_check_ms = ms + 2500; } #endif if (heating && input > temp) { if (ms > t2 + 5000) { heating = false; if (hotend < 0) soft_pwm_bed = (bias - d) >> 1; else soft_pwm[hotend] = (bias - d) >> 1; t1 = ms; t_high = t1 - t2; max = temp; } } if (!heating && input < temp) { if (ms > t1 + 5000) { heating = true; t2 = ms; t_low = t2 - t1; if (cycles > 0) { long max_pow = hotend < 0 ? MAX_BED_POWER : PID_MAX; bias += (d*(t_high - t_low))/(t_low + t_high); bias = constrain(bias, 20, max_pow - 20); d = (bias > max_pow / 2) ? max_pow - 1 - bias : bias; ECHO_MV(MSG_BIAS, bias); ECHO_MV(MSG_D, d); ECHO_MV(MSG_T_MIN, min); ECHO_MV(MSG_T_MAX, max); if (cycles > 2) { Ku = (4.0 * d) / (3.14159265 * (max - min) / 2.0); Tu = ((float)(t_low + t_high) / 1000.0); ECHO_MV(MSG_KU, Ku); ECHO_EMV(MSG_TU, Tu); Kp_temp = 0.6 * Ku; Ki_temp = 2 * Kp_temp / Tu; Kd_temp = Kp_temp * Tu / 8; ECHO_EM(MSG_CLASSIC_PID); ECHO_MV(MSG_KP, Kp_temp); ECHO_MV(MSG_KI, Ki_temp); ECHO_EMV(MSG_KD, Kd_temp); } else { ECHO_E; } } if (hotend < 0) soft_pwm_bed = (bias + d) >> 1; else soft_pwm[hotend] = (bias + d) >> 1; cycles++; min = temp; }
void RemoteControl::caminatas (byte comando) { // todo esto está programado como el orto, hay que reorganizarlo // por ejemplo este "anguloso"... talvez las otras variables también deban estar acá static float anguloso = 0; if (anguloso != 0) { COORD2D matrix [2]; getRotationMatrix (matrix, anguloso); centro_caminata = applyMatrix (centro_caminata, matrix); anguloso = 0; } // switch de seteo switch (comando) { /// mega provisorio /////////////////////////// case 5: mov.salto (velocidad, HALF_PI); break; /////////////////////////// /// termina mega provisorio case RC_UP: texto1 = "UP"; angulo = angulo_offset + HALF_PI; break; case RC_DOWN: texto1 = "DN"; angulo = angulo_offset - HALF_PI; break; case RC_LEFT: texto1 = "LEFT"; if (modo == CAMINATAS1) { angulo = angulo_offset + PI; } else { anguloso = 0; mov.mon_angulo = &anguloso; // aca el ángulo offset se usaría para rotar el centro mov.curva (velocidad, desplazamiento, (COORD2D) {0, 0} , CCW, marcha, largo_pasos); } break; case RC_RIGHT: texto1 = "RIGHT"; if (modo == CAMINATAS1) { angulo = angulo_offset; } else { anguloso = 0; mov.mon_angulo = &anguloso; // aca el ángulo offset se usaría para rotar el centro mov.curva (velocidad, desplazamiento, (COORD2D) {0, 0} , CW, marcha, largo_pasos); } break; case RC_MENU: texto1 = "MENU"; mov.mon_angulo = NULL; mov.curva (velocidad, desplazamiento, centro_caminata, CCW, marcha, largo_pasos); break; case RC_EXIT: texto1 = "EXIT"; mov.mon_angulo = NULL; mov.curva (velocidad, desplazamiento, centro_caminata, CW, marcha, largo_pasos); break; case RC_MTS: texto1 = "MTS"; mov.mon_angulo = &angulo_offset; mov.curva (velocidad, desplazamiento, centro_caminata, CCW, marcha, largo_pasos); break; case RC_CCTTX: mov.mon_angulo = &angulo_offset; mov.curva (velocidad, desplazamiento, centro_caminata, CW, marcha, largo_pasos); texto1 = "CC_TTX"; break; case RC_ENTER1: texto1 = "STOP"; mov.stop(); isMoving = false; break; case RC_VOL_UP: if (pantalla.isBusy()) {break;} velocidad = constrain (velocidad+inc, 1, 50); texto1 = "Vel " + float2string (velocidad); if (isMoving) {mov.set_vel (velocidad);} retardo = true; break; case RC_VOL_DN: if (pantalla.isBusy()) {break;} velocidad = constrain (velocidad-inc, 1, 50); texto1 = "Vel " + float2string (velocidad); if (isMoving) {mov.set_vel (velocidad);} retardo = true; break; case RC_CH_UP: if (pantalla.isBusy()) {break;} if (!isMoving) { largo_pasos = constrain (largo_pasos+inc, 0, 20); texto1 = "Paso "; if (largo_pasos == 0) {texto1 += "AUTO";} else {texto1 += float2string (largo_pasos);} } else { texto1 = "Escala " + String (mov.dec_escala(), DEC); } retardo = true; break; case RC_CH_DN: if (pantalla.isBusy()) {break;} if (!isMoving) { largo_pasos = constrain (largo_pasos-inc, 0, 20); texto1 = "Paso "; if (largo_pasos == 0) {texto1 += "AUTO";} else {texto1 += float2string (largo_pasos);} } else { texto1 = "Escala " + String (mov.inc_escala(), DEC); } retardo = true; break; } // switch de ejecución (y puede haber más; talvez la variable swicheada en segunda instancia no sea "comando") switch (comando) { case RC_UP: case RC_DOWN: case RC_LEFT: case RC_RIGHT: if (modo == CAMINATAS1) {mov.mon_desplazamiento = NULL;} else if (modo == CAMINATAS2) { if (comando == RC_LEFT || comando == RC_RIGHT) {break;} // la lógica hay que reformularla toda mov.mon_desplazamiento = ¢ro_caminata; } mov.recta (velocidad, desplazamiento, angulo, marcha, largo_pasos); isMoving = true; break; } }
void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t adjustmentFunction, int delta) { int newValue; float newFloatValue; if (delta > 0) { queueConfirmationBeep(2); } else { queueConfirmationBeep(1); } switch(adjustmentFunction) { case ADJUSTMENT_RC_RATE: newValue = (int)controlRateConfig->rcRate8 + delta; controlRateConfig->rcRate8 = constrain(newValue, 0, 250); // FIXME magic numbers repeated in serial_cli.c generatePitchRollCurve(controlRateConfig); break; case ADJUSTMENT_RC_EXPO: newValue = (int)controlRateConfig->rcExpo8 + delta; controlRateConfig->rcExpo8 = constrain(newValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c generatePitchRollCurve(controlRateConfig); break; case ADJUSTMENT_THROTTLE_EXPO: newValue = (int)controlRateConfig->thrExpo8 + delta; controlRateConfig->thrExpo8 = constrain(newValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c generateThrottleCurve(controlRateConfig, escAndServoConfig); break; case ADJUSTMENT_PITCH_ROLL_RATE: case ADJUSTMENT_PITCH_RATE: newValue = (int)controlRateConfig->rates[FD_PITCH] + delta; controlRateConfig->rates[FD_PITCH] = constrain(newValue, 0, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX); if (adjustmentFunction == ADJUSTMENT_PITCH_RATE) { break; } // follow though for combined ADJUSTMENT_PITCH_ROLL_RATE case ADJUSTMENT_ROLL_RATE: newValue = (int)controlRateConfig->rates[FD_ROLL] + delta; controlRateConfig->rates[FD_ROLL] = constrain(newValue, 0, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX); break; case ADJUSTMENT_YAW_RATE: newValue = (int)controlRateConfig->rates[FD_YAW] + delta; controlRateConfig->rates[FD_YAW] = constrain(newValue, 0, CONTROL_RATE_CONFIG_YAW_RATE_MAX); break; case ADJUSTMENT_PITCH_ROLL_P: if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) { newFloatValue = pidProfile->P_f[PIDPITCH] + (float)(delta / 10.0f); pidProfile->P_f[PIDPITCH] = constrainf(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c newFloatValue = pidProfile->P_f[PIDROLL] + (float)(delta / 10.0f); pidProfile->P_f[PIDROLL] = constrainf(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c } else { newValue = (int)pidProfile->P8[PIDPITCH] + delta; pidProfile->P8[PIDPITCH] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c newValue = (int)pidProfile->P8[PIDROLL] + delta; pidProfile->P8[PIDROLL] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c } break; case ADJUSTMENT_PITCH_ROLL_I: if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) { newFloatValue = pidProfile->I_f[PIDPITCH] + (float)(delta / 100.0f); pidProfile->I_f[PIDPITCH] = constrainf(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c newFloatValue = pidProfile->I_f[PIDROLL] + (float)(delta / 100.0f); pidProfile->I_f[PIDROLL] = constrainf(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c } else { newValue = (int)pidProfile->I8[PIDPITCH] + delta; pidProfile->I8[PIDPITCH] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c newValue = (int)pidProfile->I8[PIDROLL] + delta; pidProfile->I8[PIDROLL] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c } break; case ADJUSTMENT_PITCH_ROLL_D: if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) { newFloatValue = pidProfile->D_f[PIDPITCH] + (float)(delta / 1000.0f); pidProfile->D_f[PIDPITCH] = constrainf(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c newFloatValue = pidProfile->D_f[PIDROLL] + (float)(delta / 1000.0f); pidProfile->D_f[PIDROLL] = constrainf(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c } else { newValue = (int)pidProfile->D8[PIDPITCH] + delta; pidProfile->D8[PIDPITCH] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c newValue = (int)pidProfile->D8[PIDROLL] + delta; pidProfile->D8[PIDROLL] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c } break; case ADJUSTMENT_YAW_P: if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) { newFloatValue = pidProfile->P_f[PIDYAW] + (float)(delta / 10.0f); pidProfile->P_f[PIDYAW] = constrainf(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c } else { newValue = (int)pidProfile->P8[PIDYAW] + delta; pidProfile->P8[PIDYAW] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c } break; case ADJUSTMENT_YAW_I: if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) { newFloatValue = pidProfile->I_f[PIDYAW] + (float)(delta / 100.0f); pidProfile->I_f[PIDYAW] = constrainf(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c } else { newValue = (int)pidProfile->I8[PIDYAW] + delta; pidProfile->I8[PIDYAW] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c } break; case ADJUSTMENT_YAW_D: if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) { newFloatValue = pidProfile->D_f[PIDYAW] + (float)(delta / 1000.0f); pidProfile->D_f[PIDYAW] = constrainf(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c } else { newValue = (int)pidProfile->D8[PIDYAW] + delta; pidProfile->D8[PIDYAW] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c } break; default: break; }; }
void RemoteControl::traslaciones (byte comando) { switch (comando) { case RC_UP: texto1 = "|"; mov.translation ((COORD3D) {0, 0, step}, step/(velocidad*TICK)); break; case RC_DOWN: texto1 = "^"; mov.translation ((COORD3D) {0, 0, -step}, step/(velocidad*TICK)); break; case RC_RIGHT: texto1 = "<"; mov.translation ((COORD3D) {step, 0, 0}, step/(velocidad*TICK)); break; case RC_LEFT: texto1 = ">"; mov.translation ((COORD3D) {-step, 0, 0}, step/(velocidad*TICK)); break; case RC_CCTTX: case RC_MTS: texto1 = "h-"; mov.translation ((COORD3D) {0, -step, 0}, step/(velocidad*TICK)); break; case RC_EXIT: case RC_MENU: texto1 = "h+"; mov.translation ((COORD3D) {0, step, 0}, step/(velocidad*TICK)); // step/velocidad*TICK hay que precalcularlo break; case RC_ENTER1: texto1 = "STORE"; mov.actual_pos_ref (); break; case RC_VOL_UP: if (pantalla.isBusy()) {break;} velocidad = constrain (velocidad+inc, 1, 50); texto1 = "Vel " + float2string (velocidad); retardo = true; break; case RC_VOL_DN: if (pantalla.isBusy()) {break;} velocidad = constrain (velocidad-inc, 1, 50); texto1 = "Vel " + float2string (velocidad); retardo = true; break; case RC_CH_UP: if (pantalla.isBusy()) {break;} if (!isMoving) {step = constrain (step+inc, .5, 5);} texto1 = "Step " + float2string (step); retardo = true; break; case RC_CH_DN: if (pantalla.isBusy()) {break;} if (!isMoving) {step = constrain (step-inc, .5, 5);} texto1 = "Step " + float2string (step); retardo = true; break; } }
unsigned MultirotorMixer::mix(float *outputs, unsigned space, uint16_t *status_reg) { /* Summary of mixing strategy: 1) mix roll, pitch and thrust without yaw. 2) if some outputs violate range [0,1] then try to shift all outputs to minimize violation -> increase or decrease total thrust (boost). The total increase or decrease of thrust is limited (max_thrust_diff). If after the shift some outputs still violate the bounds then scale roll & pitch. In case there is violation at the lower and upper bound then try to shift such that violation is equal on both sides. 3) mix in yaw and scale if it leads to limit violation. 4) scale all outputs to range [idle_speed,1] */ float roll = constrain(get_control(0, 0) * _roll_scale, -1.0f, 1.0f); float pitch = constrain(get_control(0, 1) * _pitch_scale, -1.0f, 1.0f); float yaw = constrain(get_control(0, 2) * _yaw_scale, -1.0f, 1.0f); float thrust = constrain(get_control(0, 3), 0.0f, 1.0f); float min_out = 0.0f; float max_out = 0.0f; // clean register for saturation status flags if (status_reg != NULL) { (*status_reg) = 0; } // thrust boost parameters float thrust_increase_factor = 1.5f; float thrust_decrease_factor = 0.6f; /* perform initial mix pass yielding unbounded outputs, ignore yaw */ for (unsigned i = 0; i < _rotor_count; i++) { float out = roll * _rotors[i].roll_scale + pitch * _rotors[i].pitch_scale + thrust; out *= _rotors[i].out_scale; /* calculate min and max output values */ if (out < min_out) { min_out = out; } if (out > max_out) { max_out = out; } outputs[i] = out; } float boost = 0.0f; // value added to demanded thrust (can also be negative) float roll_pitch_scale = 1.0f; // scale for demanded roll and pitch if (min_out < 0.0f && max_out < 1.0f && -min_out <= 1.0f - max_out) { float max_thrust_diff = thrust * thrust_increase_factor - thrust; if (max_thrust_diff >= -min_out) { boost = -min_out; } else { boost = max_thrust_diff; roll_pitch_scale = (thrust + boost) / (thrust - min_out); } } else if (max_out > 1.0f && min_out > 0.0f && min_out >= max_out - 1.0f) { float max_thrust_diff = thrust - thrust_decrease_factor * thrust; if (max_thrust_diff >= max_out - 1.0f) { boost = -(max_out - 1.0f); } else { boost = -max_thrust_diff; roll_pitch_scale = (1 - (thrust + boost)) / (max_out - thrust); } } else if (min_out < 0.0f && max_out < 1.0f && -min_out > 1.0f - max_out) { float max_thrust_diff = thrust * thrust_increase_factor - thrust; boost = constrain(-min_out - (1.0f - max_out) / 2.0f, 0.0f, max_thrust_diff); roll_pitch_scale = (thrust + boost) / (thrust - min_out); } else if (max_out > 1.0f && min_out > 0.0f && min_out < max_out - 1.0f) { float max_thrust_diff = thrust - thrust_decrease_factor * thrust; boost = constrain(-(max_out - 1.0f - min_out) / 2.0f, -max_thrust_diff, 0.0f); roll_pitch_scale = (1 - (thrust + boost)) / (max_out - thrust); } else if (min_out < 0.0f && max_out > 1.0f) { boost = constrain(-(max_out - 1.0f + min_out) / 2.0f, thrust_decrease_factor * thrust - thrust, thrust_increase_factor * thrust - thrust); roll_pitch_scale = (thrust + boost) / (thrust - min_out); } // notify if saturation has occurred if (min_out < 0.0f) { if (status_reg != NULL) { (*status_reg) |= PX4IO_P_STATUS_MIXER_LOWER_LIMIT; } } if (max_out > 0.0f) { if (status_reg != NULL) { (*status_reg) |= PX4IO_P_STATUS_MIXER_UPPER_LIMIT; } } // mix again but now with thrust boost, scale roll/pitch and also add yaw for (unsigned i = 0; i < _rotor_count; i++) { float out = (roll * _rotors[i].roll_scale + pitch * _rotors[i].pitch_scale) * roll_pitch_scale + yaw * _rotors[i].yaw_scale + thrust + boost; out *= _rotors[i].out_scale; // scale yaw if it violates limits. inform about yaw limit reached if (out < 0.0f) { if (fabsf(_rotors[i].yaw_scale) <= FLT_EPSILON) { yaw = 0.0f; } else { yaw = -((roll * _rotors[i].roll_scale + pitch * _rotors[i].pitch_scale) * roll_pitch_scale + thrust + boost) / _rotors[i].yaw_scale; } if (status_reg != NULL) { (*status_reg) |= PX4IO_P_STATUS_MIXER_YAW_LIMIT; } } else if (out > 1.0f) { // allow to reduce thrust to get some yaw response float thrust_reduction = fminf(0.15f, out - 1.0f); thrust -= thrust_reduction; if (fabsf(_rotors[i].yaw_scale) <= FLT_EPSILON) { yaw = 0.0f; } else { yaw = (1.0f - ((roll * _rotors[i].roll_scale + pitch * _rotors[i].pitch_scale) * roll_pitch_scale + thrust + boost)) / _rotors[i].yaw_scale; } if (status_reg != NULL) { (*status_reg) |= PX4IO_P_STATUS_MIXER_YAW_LIMIT; } } } /* add yaw and scale outputs to range idle_speed...1 */ for (unsigned i = 0; i < _rotor_count; i++) { outputs[i] = (roll * _rotors[i].roll_scale + pitch * _rotors[i].pitch_scale) * roll_pitch_scale + yaw * _rotors[i].yaw_scale + thrust + boost; outputs[i] = constrain(_idle_speed + (outputs[i] * (1.0f - _idle_speed)), _idle_speed, 1.0f); } return _rotor_count; }
void RemoteControl::rotaciones (byte comando) { switch (comando) { case RC_UP: texto1 = "|"; mov.rotation (-step/15, 0, 0, step/(velocidad*TICK)); // radio = 15 break; case RC_DOWN: texto1 = "^"; mov.rotation (step/15, 0, 0, step/(velocidad*TICK)); // radio = 15 break; case RC_RIGHT: texto1 = "<"; mov.rotation (0, 0, step/15, step/(velocidad*TICK)); // radio = 15 break; case RC_LEFT: texto1 = ">"; mov.rotation (0, 0, -step/15, step/(velocidad*TICK)); // radio = 15 break; case RC_MENU: case RC_MTS: texto1 = "h-"; mov.rotation (0, step/15, 0, step/(velocidad*TICK)); // radio = 15 break; case RC_EXIT: case RC_CCTTX: texto1 = "h+"; mov.rotation (0, -step/15, 0, step/(velocidad*TICK)); // radio = 15 break; // de acá al final es exactamente lo mismo que "traslaciones"... lo de arriba es bastante parecido también case RC_ENTER1: texto1 = "STORE"; mov.actual_pos_ref (); break; case RC_VOL_UP: if (pantalla.isBusy()) {break;} velocidad = constrain (velocidad+inc, 1, 50); texto1 = "Vel " + float2string (velocidad); retardo = true; break; case RC_VOL_DN: if (pantalla.isBusy()) {break;} velocidad = constrain (velocidad-inc, 1, 50); texto1 = "Vel " + float2string (velocidad); retardo = true; break; case RC_CH_UP: if (pantalla.isBusy()) {break;} if (!isMoving) {step = constrain (step+inc, .5, 5);} texto1 = "Step " + float2string (step); retardo = true; break; case RC_CH_DN: if (pantalla.isBusy()) {break;} if (!isMoving) {step = constrain (step-inc, .5, 5);} texto1 = "Step " + float2string (step); retardo = true; break; } }
static void sendFuelLevel(void) { sendDataHead(ID_FUEL_LEVEL); serialize16((uint16_t)constrain(mAhDrawn, 0, 0xFFFF)); }
void RemoteControl::editar_centro (byte comando) { switch (comando) { case RC_UP: if (pantalla.isBusy()) {break;} mov.bd.centro_ref.z += step; texto1 = "Z = " + float2string (mov.bd.centro_ref.z); retardo = true; break; case RC_DOWN: if (pantalla.isBusy()) {break;} mov.bd.centro_ref.z -= step; texto1 = "Z = " + float2string (mov.bd.centro_ref.z); retardo = true; break; case RC_RIGHT: if (pantalla.isBusy()) {break;} mov.bd.centro_ref.x += step; texto1 = "X = " + float2string (mov.bd.centro_ref.x); retardo = true; break; case RC_LEFT: if (pantalla.isBusy()) {break;} mov.bd.centro_ref.x -= step; texto1 = "X = " + float2string (mov.bd.centro_ref.x); retardo = true; break; case RC_CCTTX: case RC_MTS: if (pantalla.isBusy()) {break;} mov.bd.centro_ref.y -= step; texto1 = "Y = " + float2string (mov.bd.centro_ref.y); retardo = true; break; case RC_EXIT: case RC_MENU: if (pantalla.isBusy()) {break;} mov.bd.centro_ref.y += step; texto1 = "Y = " + float2string (mov.bd.centro_ref.y); retardo = true; break; // esto también se repite case RC_CH_UP: if (pantalla.isBusy()) {break;} if (!isMoving) {step = constrain (step+inc, .5, 5);} texto1 = "Step " + float2string (step); retardo = true; break; case RC_CH_DN: if (pantalla.isBusy()) {break;} if (!isMoving) {step = constrain (step-inc, .5, 5);} texto1 = "Step " + float2string (step); retardo = true; break; } }
static void pidMultiWii(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig) { UNUSED(rxConfig); int axis, prop; int32_t error, errorAngle; int32_t PTerm, ITerm, PTermACC = 0, ITermACC = 0, PTermGYRO = 0, ITermGYRO = 0, DTerm; static int16_t lastGyro[3] = { 0, 0, 0 }; static int32_t delta1[3], delta2[3]; int32_t deltaSum; int32_t delta; UNUSED(controlRateConfig); // **** PITCH & ROLL & YAW PID **** prop = MIN(MAX(ABS(rcCommand[PITCH]), ABS(rcCommand[ROLL])), 500); // range [0;500] for (axis = 0; axis < 3; axis++) { if ((FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) && (axis == FD_ROLL || axis == FD_PITCH)) { // MODE relying on ACC // observe max inclination #ifdef GPS errorAngle = constrain(2 * rcCommand[axis] + GPS_angle[axis], -((int) max_angle_inclination), +max_angle_inclination) - inclination.raw[axis] + angleTrim->raw[axis]; #else errorAngle = constrain(2 * rcCommand[axis], -((int) max_angle_inclination), +max_angle_inclination) - inclination.raw[axis] + angleTrim->raw[axis]; #endif #ifdef AUTOTUNE if (shouldAutotune()) { errorAngle = DEGREES_TO_DECIDEGREES(autotune(rcAliasToAngleIndexMap[axis], &inclination, DECIDEGREES_TO_DEGREES(errorAngle))); } #endif PTermACC = errorAngle * pidProfile->P8[PIDLEVEL] / 100; // 32 bits is needed for calculation: errorAngle*P8[PIDLEVEL] could exceed 32768 16 bits is ok for result PTermACC = constrain(PTermACC, -pidProfile->D8[PIDLEVEL] * 5, +pidProfile->D8[PIDLEVEL] * 5); errorAngleI[axis] = constrain(errorAngleI[axis] + errorAngle, -10000, +10000); // WindUp ITermACC = (errorAngleI[axis] * pidProfile->I8[PIDLEVEL]) >> 12; } if (!FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE) || axis == FD_YAW) { // MODE relying on GYRO or YAW axis error = (int32_t) rcCommand[axis] * 10 * 8 / pidProfile->P8[axis]; error -= gyroData[axis] / 4; PTermGYRO = rcCommand[axis]; errorGyroI[axis] = constrain(errorGyroI[axis] + error, -16000, +16000); // WindUp if ((ABS(gyroData[axis]) > (640 * 4)) || (axis == FD_YAW && ABS(rcCommand[axis]) > 100)) errorGyroI[axis] = 0; ITermGYRO = (errorGyroI[axis] / 125 * pidProfile->I8[axis]) / 64; } if (FLIGHT_MODE(HORIZON_MODE) && (axis == FD_ROLL || axis == FD_PITCH)) { PTerm = (PTermACC * (500 - prop) + PTermGYRO * prop) / 500; ITerm = (ITermACC * (500 - prop) + ITermGYRO * prop) / 500; } else { if (FLIGHT_MODE(ANGLE_MODE) && (axis == FD_ROLL || axis == FD_PITCH)) { PTerm = PTermACC; ITerm = ITermACC; } else { PTerm = PTermGYRO; ITerm = ITermGYRO; } } PTerm -= ((int32_t)gyroData[axis] / 4) * dynP8[axis] / 10 / 8; // 32 bits is needed for calculation delta = (gyroData[axis] - lastGyro[axis]) / 4; lastGyro[axis] = gyroData[axis]; deltaSum = delta1[axis] + delta2[axis] + delta; delta2[axis] = delta1[axis]; delta1[axis] = delta; DTerm = (deltaSum * dynD8[axis]) / 32; axisPID[axis] = PTerm + ITerm - DTerm; #ifdef BLACKBOX axisPID_P[axis] = PTerm; axisPID_I[axis] = ITerm; axisPID_D[axis] = -DTerm; #endif }
void look_for_lines_to_connect() { float sx, sy, ex, ey; for (uint8_t i = 0; i < GRID_MAX_POINTS_X; i++) { for (uint8_t j = 0; j < GRID_MAX_POINTS_Y; j++) { if (i < GRID_MAX_POINTS_X) { // We can't connect to anything to the right than GRID_MAX_POINTS_X. // This is already a half circle because we are at the edge of the bed. if (is_bit_set(circle_flags, i, j) && is_bit_set(circle_flags, i + 1, j)) { // check if we can do a line to the left if (!is_bit_set(horizontal_mesh_line_flags, i, j)) { // // We found two circles that need a horizontal line to connect them // Print it! // sx = pgm_read_float(&ubl.mesh_index_to_xpos[ i ]) + (SIZE_OF_INTERSECTION_CIRCLES - (SIZE_OF_CROSSHAIRS)); // right edge ex = pgm_read_float(&ubl.mesh_index_to_xpos[i + 1]) - (SIZE_OF_INTERSECTION_CIRCLES - (SIZE_OF_CROSSHAIRS)); // left edge sx = constrain(sx, X_MIN_POS + 1, X_MAX_POS - 1); sy = ey = constrain(pgm_read_float(&ubl.mesh_index_to_ypos[j]), Y_MIN_POS + 1, Y_MAX_POS - 1); ex = constrain(ex, X_MIN_POS + 1, X_MAX_POS - 1); if (ubl.g26_debug_flag) { SERIAL_ECHOPAIR(" Connecting with horizontal line (sx=", sx); SERIAL_ECHOPAIR(", sy=", sy); SERIAL_ECHOPAIR(") -> (ex=", ex); SERIAL_ECHOPAIR(", ey=", ey); SERIAL_CHAR(')'); SERIAL_EOL; //debug_current_and_destination(PSTR("Connecting horizontal line.")); } print_line_from_here_to_there(LOGICAL_X_POSITION(sx), LOGICAL_Y_POSITION(sy), layer_height, LOGICAL_X_POSITION(ex), LOGICAL_Y_POSITION(ey), layer_height); bit_set(horizontal_mesh_line_flags, i, j); // Mark it as done so we don't do it again } } if (j < GRID_MAX_POINTS_Y) { // We can't connect to anything further back than GRID_MAX_POINTS_Y. // This is already a half circle because we are at the edge of the bed. if (is_bit_set(circle_flags, i, j) && is_bit_set(circle_flags, i, j + 1)) { // check if we can do a line straight down if (!is_bit_set( vertical_mesh_line_flags, i, j)) { // // We found two circles that need a vertical line to connect them // Print it! // sy = pgm_read_float(&ubl.mesh_index_to_ypos[ j ]) + (SIZE_OF_INTERSECTION_CIRCLES - (SIZE_OF_CROSSHAIRS)); // top edge ey = pgm_read_float(&ubl.mesh_index_to_ypos[j + 1]) - (SIZE_OF_INTERSECTION_CIRCLES - (SIZE_OF_CROSSHAIRS)); // bottom edge sx = ex = constrain(pgm_read_float(&ubl.mesh_index_to_xpos[i]), X_MIN_POS + 1, X_MAX_POS - 1); sy = constrain(sy, Y_MIN_POS + 1, Y_MAX_POS - 1); ey = constrain(ey, Y_MIN_POS + 1, Y_MAX_POS - 1); if (ubl.g26_debug_flag) { SERIAL_ECHOPAIR(" Connecting with vertical line (sx=", sx); SERIAL_ECHOPAIR(", sy=", sy); SERIAL_ECHOPAIR(") -> (ex=", ex); SERIAL_ECHOPAIR(", ey=", ey); SERIAL_CHAR(')'); SERIAL_EOL; debug_current_and_destination(PSTR("Connecting vertical line.")); } print_line_from_here_to_there(LOGICAL_X_POSITION(sx), LOGICAL_Y_POSITION(sy), layer_height, LOGICAL_X_POSITION(ex), LOGICAL_Y_POSITION(ey), layer_height); bit_set(vertical_mesh_line_flags, i, j); // Mark it as done so we don't do it again } } } } } } }