// 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();
}
void motorsCommands(float throtCommand, float pitchCommand, float rollCommand, float ruddCommand) {
float throttle;
float voltageFactor;
float value;
float nominalBatVolts;
int i;
// throttle limiter to prevent control saturation
throttle = constrainFloat(throtCommand - motorsData.throttleLimiter, 0.0f, MOTORS_SCALE);
// calculate voltage factor
if (p[CTRL_BATT_COMP > 0.0]) {
nominalBatVolts = MOTORS_CELL_VOLTS*analogData.batCellCount;
voltageFactor = 1.0f + (nominalBatVolts - analogData.vIn) / nominalBatVolts * p[CTRL_BATT_COMP];
}
// calculate and set each motor value
for (i = 0; i < MOTORS_NUM; i++) {
if (motorsData.active[i]) {
motorsPowerStruct_t *d = &motorsData.distribution[i];
value = 0.0f;
value += (throttle * d->throttle * 0.01f);
value += (pitchCommand * d->pitch * 0.01f);
value += (rollCommand * d->roll * 0.01f);
value += (ruddCommand * d->yaw * 0.01f);
if (p[CTRL_BATT_COMP > 0.0]) {
value *= voltageFactor;
}
// check for over throttle
if (value >= MOTORS_SCALE) {
motorsData.throttleLimiter += MOTORS_THROTTLE_LIMITER;
}
motorsData.value[i] = constrainInt(value, 0, MOTORS_SCALE);
}
}
motorsSendValues();
// decay throttle limit
motorsData.throttleLimiter = constrainFloat(motorsData.throttleLimiter - MOTORS_THROTTLE_LIMITER, 0.0f, MOTORS_SCALE/4);
motorsData.pitch = pitchCommand;
motorsData.roll = rollCommand;
motorsData.yaw = ruddCommand;
motorsData.throttle = throttle;
}
// 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();
}
void motorsCommands(float throtCommand, float pitchCommand, float rollCommand, float ruddCommand) {
float throttle;
float value;
int i;
// throttle limiter to prevent control saturation
throttle = constrainFloat(throtCommand - motorsData.throttleLimiter, 0.0f, MOTORS_SCALE);
// calculate and set each motor value
for (int j = 0; j < motorsData.numActive; ++j) {
i = motorsData.activeList[j];
motorsPowerStruct_t *d = &motorsData.distribution[i];
value = 0.0f;
value += (throttle * d->throttle * 0.01f);
value += (pitchCommand * d->pitch * 0.01f);
value += (rollCommand * d->roll * 0.01f);
value += (ruddCommand * d->yaw * 0.01f);
// adjust for voltage factor
value *= motorsVFactor();
// check for over throttle
if (value >= MOTORS_SCALE)
motorsData.throttleLimiter += MOTORS_THROTTLE_LIMITER;
motorsData.value[i] = constrainInt(value, 0, MOTORS_SCALE);
}
motorsSendValues();
// decay throttle limit
motorsData.throttleLimiter = constrainFloat(motorsData.throttleLimiter - MOTORS_THROTTLE_LIMITER, 0.0f, MOTORS_SCALE/4);
motorsData.pitch = pitchCommand;
motorsData.roll = rollCommand;
motorsData.yaw = ruddCommand;
motorsData.throttle = throttle;
}
