void gain_scheduling_periodic(void)
{
#if NUMBER_OF_GAINSETS > 1
uint8_t section = 0;
//Find out between which gainsets to interpolate
while (FLOAT_OF_BFP(SCHEDULING_VARIABLE, SCHEDULING_VARIABLE_FRAC) > scheduling_points[section]) {
section++;
if (section == NUMBER_OF_GAINSETS) { break; }
}
//Get pointers for the two gainsets and the stabilization_gains
struct Int32AttitudeGains *ga, *gb, *gblend;
gblend = &stabilization_gains;
if (section == 0) {
set_gainset(0);
} else if (section == NUMBER_OF_GAINSETS) {
set_gainset(NUMBER_OF_GAINSETS - 1);
} else {
ga = &gainlibrary[section - 1];
gb = &gainlibrary[section];
//Calculate the ratio between the scheduling points
int32_t ratio;
ratio = BFP_OF_REAL((FLOAT_OF_BFP(SCHEDULING_VARIABLE,
SCHEDULING_VARIABLE_FRAC) - scheduling_points[section - 1]) / (scheduling_points[section] -
scheduling_points[section - 1]), INT32_RATIO_FRAC);
int64_t g1, g2, gbl;
//Loop through the gains and interpolate
for (int i = 0; i < (sizeof(struct Int32AttitudeGains) / sizeof(int32_t)); i++) {
g1 = *(((int32_t *) ga) + i);
g1 *= (1 << INT32_RATIO_FRAC) - ratio;
g2 = *(((int32_t *) gb) + i);
g2 *= ratio;
gbl = (g1 + g2) >> INT32_RATIO_FRAC;
*(((int32_t *) gblend) + i) = (int32_t) gbl;
}
}
#endif
}
void ctrl_eff_scheduling_periodic(void)
{
// Go from transition percentage to ratio
float ratio = FLOAT_OF_BFP(transition_percentage, INT32_PERCENTAGE_FRAC) / 100;
int8_t i;
int8_t j;
for (i = 0; i < INDI_OUTPUTS; i++) {
for (j = 0; j < INDI_NUM_ACT; j++) {
g1g2[i][j] = (g1g2_hover[i][j] * (1.0 - ratio) + g1g2_forward[i][j] * ratio);
}
}
}
