Пример #1
0
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;
}
Пример #2
0
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]);
}
Пример #3
0
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
    }
}
Пример #4
0
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;
  }
}
Пример #5
0
float PID::get_p_error_lim(float targetInput, float actualInput){
	_error = targetInput - actualInput;
	_error = constrain(_error,-_pMax._value,_pMax._value);
	return _error;
}
Пример #6
0
void LinearActuator::goToPosition(uint16_t target_position) {
  _target_position = constrain(target_position,
                               LA_RAW_POS_MIN, LA_RAW_POS_MAX);
}
Пример #7
0
float Ramp::progress() const {
	// Compute progress as % of duration.
	float prog = elapsed() / _duration;
	prog = constrain(prog, 0, 1);
	return prog;
}
Пример #8
0
	/**************************************************************************
	* \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);
	  //}
	}
Пример #9
0
	float ray2::get_closest_t_constrained(const vec2& p) const {
		return constrain(0.f, 1.f, get_closest_t_unconstrained(p));
	}
Пример #10
0
/*******************************************************************************
**   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
Пример #11
0
uint8_t calculateBatteryCapacityRemainingPercentage(void)
{
    uint16_t batteryCapacity = batteryConfig()->batteryCapacity;

    return constrain((batteryCapacity - constrain(mAhDrawn, 0, 0xFFFF)) * 100.0f / batteryCapacity , 0, 100);
}
Пример #12
0
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();
}
Пример #13
0
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;
}
Пример #14
0
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);
    }
}
Пример #15
0
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;
			
	}
	
}
Пример #16
0
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;
}
Пример #17
0
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 ();
	
}
Пример #18
0
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;
}
Пример #19
0
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);
}
Пример #20
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);
}
Пример #21
0
  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;
          }
Пример #22
0
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 = &centro_caminata;
			}
		  mov.recta (velocidad, desplazamiento, angulo, marcha, largo_pasos);
			isMoving = true;
			break;
	}

}
Пример #23
0
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;
    };
}
Пример #24
0
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;
	}

}
Пример #25
0
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;
}
Пример #26
0
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;
	}
}
Пример #27
0
static void sendFuelLevel(void)
{
    sendDataHead(ID_FUEL_LEVEL);
    serialize16((uint16_t)constrain(mAhDrawn, 0, 0xFFFF));
}
Пример #28
0
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;
	}
}
Пример #29
0
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
    }
Пример #30
0
  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
              }
            }
          }
        }
      }
    }
  }