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