C++ (Cpp) motorsThrust2Value示例

示例#1

文件： motors.c 项目： mpaperno/aq_flight_control

// thrust in gram-force
void motorsSendThrust(void) {
    float value, v;
    int i;

    for (int j = 0; j < motorsData.numActive; ++j) {
	i = motorsData.activeList[j];
	value = motorsThrust2Value(motorsData.thrust[i]);

	// using open-loop PWM ESC?
	if (i < PWM_NUM_PORTS && motorsData.pwm[i] && (uint8_t)p[MOT_ESC_TYPE] != ESC_TYPE_ESC32) {
	    // preload the request to accelerate setpoint changes
	    if (motorsData.oldValues[i] != value) {
		v = (value -  motorsData.oldValues[i]);

		// increase
		if (v > 0.0f)
		    value += v * MOTORS_COMP_PRELOAD_PTERM;
		// decrease
		else
		    value += v * MOTORS_COMP_PRELOAD_PTERM * MOTORS_COMP_PRELOAD_NFACT;

		// slowly follow setpoint
		motorsData.oldValues[i] += v * MOTORS_COMP_PRELOAD_TAU;
	    }

	    // adjust for voltage factor
	    value *= motorsVFactor();
	}

	motorsData.value[i] = constrainInt(value * MOTORS_SCALE / p[MOT_VALUE_SCAL], 0, MOTORS_SCALE);
    }

    motorsSendValues();
}

示例#2

文件： motors.c 项目： tenyan/quadfork

// thrust in gram-force
void motorsSendThrust(void) {
	float value;
#ifdef HAS_ONBOARD_ESC
	float nominalBatVolts, voltageFactor;
#endif
	int i;

	for (i = 0; i < MOTORS_NUM; i++) {
		if (motorsData.active[i]) {
			value = motorsThrust2Value(motorsData.thrust[i]);

#ifdef HAS_ONBOARD_ESC
			if (motorsData.pwm[i]) {
				// preload the request to accelerate setpoint changes
				if (motorsData.oldValues[i] != value) {
					float v = (value -  motorsData.oldValues[i]);

					// increase
					if (v > 0.0f) {
						value += v * MOTORS_COMP_PRELOAD_PTERM;
					}
					// decrease
					else {
						value += v * MOTORS_COMP_PRELOAD_PTERM * MOTORS_COMP_PRELOAD_NFACT;
					}

					// slowly follow setpoint
					motorsData.oldValues[i] += v * MOTORS_COMP_PRELOAD_TAU;
				}

				// battery voltage compensation
				if (p[CTRL_BATT_COMP] > 0.0) {
					nominalBatVolts = MOTORS_CELL_VOLTS * analogData.batCellCount;
					voltageFactor = 1.0f + (nominalBatVolts - analogData.vIn) / nominalBatVolts * p[CTRL_BATT_COMP];
					value *= voltageFactor;
				}
			}
#endif

			motorsData.value[i] = constrainInt(value * MOTORS_SCALE / p[MOT_VALUE_SCAL], 0, MOTORS_SCALE);
		}
	}

	motorsSendValues();
}

示例#3

文件： motors.c 项目： caoqing32/aq_flight_control

// thrust in gram-force
void motorsSendThrust(void) {
    float value;
#ifdef HAS_ONBOARD_ESC
    float nominalBatVolts, voltageFactor;
#endif
    int i;

    for (i = 0; i < MOTORS_NUM; i++) {
	if (motorsData.active[i]) {
	    value = motorsThrust2Value(motorsData.thrust[i]);

#ifdef HAS_ONBOARD_ESC
	    if (motorsData.pwm[i]) {
		// preload the request to accelerate setpoint changes
		if (motorsData.oldValues[i] != value) {
		    float v = (value -  motorsData.oldValues[i]);

		    // increase
		    if (v > 0.0f)
			value += v * MOTORS_COMP_PRELOAD_PTERM;
		    // decrease
		    else
			value += v * MOTORS_COMP_PRELOAD_PTERM * MOTORS_COMP_PRELOAD_NFACT;

		    // slowly follow setpoint
		    motorsData.oldValues[i] += v * MOTORS_COMP_PRELOAD_TAU;
		}

		// battery voltage compensation
		nominalBatVolts = MOTORS_CELL_VOLTS * analogData.batCellCount;
		//Express voltage command as fraction of battery volts & prevent possible division by 0 during startup
		if (analogData.vIn > nominalBatVolts*0.5f)
		    value /= analogData.vIn;
	    }
#else
	    value /= p[MOT_VALUE_SCAL];
#endif

	    motorsData.value[i] = constrainInt(value * MOTORS_SCALE, 0, MOTORS_SCALE);//scale output rpm or voltage from 0 to MOTORS_SCALE
	}
    }

    motorsSendValues();
}