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