Ejemplo n.º 1
0
/**
 * Called after measuring roll and pitch to update the
 * stabilization settings
 * 
 * takes in @ref RelayTuning and outputs @ref StabilizationSettings
 */
static void update_stabilization_settings()
{
	RelayTuningData relayTuning;
	RelayTuningGet(&relayTuning);

	RelayTuningSettingsData relaySettings;
	RelayTuningSettingsGet(&relaySettings);

	StabilizationSettingsData stabSettings;
	StabilizationSettingsGet(&stabSettings);

	// Eventually get these settings from RelayTuningSettings
	const float gain_ratio_r = 1.0f / 3.0f;
	const float zero_ratio_r = 1.0f / 3.0f;
	const float gain_ratio_p = 1.0f / 5.0f;
	const float zero_ratio_p = 1.0f / 5.0f;

	// For now just run over roll and pitch
	for (uint32_t i = 0; i < 2; i++) {
		float wu = 1000.0f * 2 * PI / relayTuning.Period[i]; // ultimate freq = output osc freq (rad/s)

		float wc = wu * gain_ratio_r;      // target openloop crossover frequency (rad/s)
		float zc = wc * zero_ratio_r;      // controller zero location (rad/s)
		float kpu = 4.0f / PI / relayTuning.Gain[i];  // ultimate gain, i.e. the proportional gain for instablity
		float kp = kpu * gain_ratio_r;     // proportional gain
		float ki = zc * kp;                // integral gain

		// Now calculate gains for the next loop out knowing it is the integral of
	 	// the inner loop -- the plant is position/velocity = scale*1/s
		float wc2 = wc * gain_ratio_p;          // crossover of the attitude loop
		float kp2 = wc2;                       // kp of attitude
		float ki2 = wc2 * zero_ratio_p * kp2;  // ki of attitude

		switch(i) {
			case 0: // roll
				stabSettings.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KP] = kp;
				stabSettings.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KI] = ki;
				stabSettings.RollPI[STABILIZATIONSETTINGS_ROLLPI_KP] = kp2;
				stabSettings.RollPI[STABILIZATIONSETTINGS_ROLLPI_KI] = ki2;
				break;
			case 1: // Pitch
				stabSettings.PitchRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KP] = kp;
				stabSettings.PitchRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KI] = ki;
				stabSettings.PitchPI[STABILIZATIONSETTINGS_ROLLPI_KP] = kp2;
				stabSettings.PitchPI[STABILIZATIONSETTINGS_ROLLPI_KI] = ki2;
				break;
			default:
				// Oh shit oh shit oh shit
				break;
		}
	}
	switch(relaySettings.Behavior) {
		case RELAYTUNINGSETTINGS_BEHAVIOR_MEASURE:
			// Just measure, don't update the stab settings
			break;
		case RELAYTUNINGSETTINGS_BEHAVIOR_COMPUTE:
			StabilizationSettingsSet(&stabSettings);
			break;
		case RELAYTUNINGSETTINGS_BEHAVIOR_SAVE:
			StabilizationSettingsSet(&stabSettings);
			UAVObjSave(StabilizationSettingsHandle(), 0);
			break;
	}
	
}
Ejemplo n.º 2
0
/**
 * Initialise the module, called on startup
 * \returns 0 on success or -1 if initialisation failed
 */
int32_t TxPIDInitialize(void)
{
	bool module_enabled;

#ifdef MODULE_TxPID_BUILTIN
	module_enabled = true;
#else
	uint8_t module_state[MODULESETTINGS_ADMINSTATE_NUMELEM];
	ModuleSettingsAdminStateGet(module_state);
	if (module_state[MODULESETTINGS_ADMINSTATE_TXPID] == MODULESETTINGS_ADMINSTATE_ENABLED) {
		module_enabled = true;
	} else {
		module_enabled = false;
	}
#endif

	if (module_enabled) {

		TxPIDSettingsInitialize();
		AccessoryDesiredInitialize();

		UAVObjEvent ev = {
			.obj = AccessoryDesiredHandle(),
			.instId = 0,
			.event = 0,
		};
		EventPeriodicCallbackCreate(&ev, updatePIDs, SAMPLE_PERIOD_MS);

#if (TELEMETRY_UPDATE_PERIOD_MS != 0)
		// Change StabilizationSettings update rate from OnChange to periodic
		// to prevent telemetry link flooding with frequent updates in case of
		// control channel jitter.
		// Warning: saving to flash with this code active will change the
		// StabilizationSettings update rate permanently. Use Metadata via
		// browser to reset to defaults (telemetryAcked=true, OnChange).
		UAVObjMetadata metadata;
		StabilizationSettingsInitialize();
		StabilizationSettingsGetMetadata(&metadata);
		metadata.telemetryAcked = 0;
		metadata.telemetryUpdateMode = UPDATEMODE_PERIODIC;
		metadata.telemetryUpdatePeriod = TELEMETRY_UPDATE_PERIOD_MS;
		StabilizationSettingsSetMetadata(&metadata);
#endif

		return 0;
	}

	return -1;
}

MODULE_INITCALL(TxPIDInitialize, NULL);

/**
 * Update PIDs callback function
 */
static void updatePIDs(UAVObjEvent* ev)
{
	if (ev->obj != AccessoryDesiredHandle())
		return;

	TxPIDSettingsData inst;
	TxPIDSettingsGet(&inst);

	if (inst.UpdateMode == TXPIDSETTINGS_UPDATEMODE_NEVER)
		return;

	uint8_t armed;
	FlightStatusArmedGet(&armed);
	if ((inst.UpdateMode == TXPIDSETTINGS_UPDATEMODE_WHENARMED) &&
			(armed == FLIGHTSTATUS_ARMED_DISARMED))
		return;

	StabilizationSettingsData stab;
	StabilizationSettingsGet(&stab);

#ifdef UAVOBJ_INIT_vtolpathfollowersettings
	VtolPathFollowerSettingsData vtolPathFollowerSettingsData;
	// Check to make sure the settings UAVObject has been instantiated
	if (VtolPathFollowerSettingsHandle()) {
		VtolPathFollowerSettingsGet(&vtolPathFollowerSettingsData);
	}

	bool vtolPathFollowerSettingsNeedsUpdate = false;
#endif

	AccessoryDesiredData accessory;

	bool stabilizationSettingsNeedsUpdate = false;

	// Loop through every enabled instance
	for (uint8_t i = 0; i < TXPIDSETTINGS_PIDS_NUMELEM; i++) {
		if (inst.PIDs[i] != TXPIDSETTINGS_PIDS_DISABLED) {

			float value;
			if (inst.Inputs[i] == TXPIDSETTINGS_INPUTS_THROTTLE) {
				ManualControlCommandThrottleGet(&value);
				value = scale(value,
						inst.ThrottleRange[TXPIDSETTINGS_THROTTLERANGE_MIN],
						inst.ThrottleRange[TXPIDSETTINGS_THROTTLERANGE_MAX],
						inst.MinPID[i], inst.MaxPID[i]);
			} else if (AccessoryDesiredInstGet(inst.Inputs[i] - TXPIDSETTINGS_INPUTS_ACCESSORY0, &accessory) == 0) {
				value = scale(accessory.AccessoryVal, -1.0, 1.0, inst.MinPID[i], inst.MaxPID[i]);
			} else {
				continue;
			}

			switch (inst.PIDs[i]) {
			case TXPIDSETTINGS_PIDS_ROLLRATEKP:
				stabilizationSettingsNeedsUpdate |= update(&stab.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KP], value);
				break;
			case TXPIDSETTINGS_PIDS_ROLLRATEKI:
				stabilizationSettingsNeedsUpdate |= update(&stab.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KI], value);
				break;
			case TXPIDSETTINGS_PIDS_ROLLRATEKD:
				stabilizationSettingsNeedsUpdate |= update(&stab.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KD], value);
				break;
			case TXPIDSETTINGS_PIDS_ROLLRATEILIMIT:
				stabilizationSettingsNeedsUpdate |= update(&stab.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_ILIMIT], value);
				break;
			case TXPIDSETTINGS_PIDS_ROLLATTITUDEKP:
				stabilizationSettingsNeedsUpdate |= update(&stab.RollPI[STABILIZATIONSETTINGS_ROLLPI_KP], value);
				break;
			case TXPIDSETTINGS_PIDS_ROLLATTITUDEKI:
				stabilizationSettingsNeedsUpdate |= update(&stab.RollPI[STABILIZATIONSETTINGS_ROLLPI_KI], value);
				break;
			case TXPIDSETTINGS_PIDS_ROLLATTITUDEILIMIT:
				stabilizationSettingsNeedsUpdate |= update(&stab.RollPI[STABILIZATIONSETTINGS_ROLLPI_ILIMIT], value);
				break;
			case TXPIDSETTINGS_PIDS_PITCHRATEKP:
				stabilizationSettingsNeedsUpdate |= update(&stab.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KP], value);
				break;
			case TXPIDSETTINGS_PIDS_PITCHRATEKI:
				stabilizationSettingsNeedsUpdate |= update(&stab.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KI], value);
				break;
			case TXPIDSETTINGS_PIDS_PITCHRATEKD:
				stabilizationSettingsNeedsUpdate |= update(&stab.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KD], value);
				break;
			case TXPIDSETTINGS_PIDS_PITCHRATEILIMIT:
				stabilizationSettingsNeedsUpdate |= update(&stab.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_ILIMIT], value);
				break;
			case TXPIDSETTINGS_PIDS_PITCHATTITUDEKP:
				stabilizationSettingsNeedsUpdate |= update(&stab.PitchPI[STABILIZATIONSETTINGS_PITCHPI_KP], value);
				break;
			case TXPIDSETTINGS_PIDS_PITCHATTITUDEKI:
				stabilizationSettingsNeedsUpdate |= update(&stab.PitchPI[STABILIZATIONSETTINGS_PITCHPI_KI], value);
				break;
			case TXPIDSETTINGS_PIDS_PITCHATTITUDEILIMIT:
				stabilizationSettingsNeedsUpdate |= update(&stab.PitchPI[STABILIZATIONSETTINGS_PITCHPI_ILIMIT], value);
				break;
			case TXPIDSETTINGS_PIDS_ROLLPITCHRATEKP:
				stabilizationSettingsNeedsUpdate |= update(&stab.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KP], value);
				stabilizationSettingsNeedsUpdate |= update(&stab.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KP], value);
				break;
			case TXPIDSETTINGS_PIDS_ROLLPITCHRATEKI:
				stabilizationSettingsNeedsUpdate |= update(&stab.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KI], value);
				stabilizationSettingsNeedsUpdate |= update(&stab.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KI], value);
				break;
			case TXPIDSETTINGS_PIDS_ROLLPITCHRATEKD:
				stabilizationSettingsNeedsUpdate |= update(&stab.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KD], value);
				stabilizationSettingsNeedsUpdate |= update(&stab.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KD], value);
				break;
			case TXPIDSETTINGS_PIDS_ROLLPITCHRATEILIMIT:
				stabilizationSettingsNeedsUpdate |= update(&stab.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_ILIMIT], value);
				stabilizationSettingsNeedsUpdate |= update(&stab.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_ILIMIT], value);
				break;
			case TXPIDSETTINGS_PIDS_ROLLPITCHATTITUDEKP:
				stabilizationSettingsNeedsUpdate |= update(&stab.RollPI[STABILIZATIONSETTINGS_ROLLPI_KP], value);
				stabilizationSettingsNeedsUpdate |= update(&stab.PitchPI[STABILIZATIONSETTINGS_PITCHPI_KP], value);
				break;
			case TXPIDSETTINGS_PIDS_ROLLPITCHATTITUDEKI:
				stabilizationSettingsNeedsUpdate |= update(&stab.RollPI[STABILIZATIONSETTINGS_ROLLPI_KI], value);
				stabilizationSettingsNeedsUpdate |= update(&stab.PitchPI[STABILIZATIONSETTINGS_PITCHPI_KI], value);
				break;
			case TXPIDSETTINGS_PIDS_ROLLPITCHATTITUDEILIMIT:
				stabilizationSettingsNeedsUpdate |= update(&stab.RollPI[STABILIZATIONSETTINGS_ROLLPI_ILIMIT], value);
				stabilizationSettingsNeedsUpdate |= update(&stab.PitchPI[STABILIZATIONSETTINGS_PITCHPI_ILIMIT], value);
				break;
			case TXPIDSETTINGS_PIDS_YAWRATEKP:
				stabilizationSettingsNeedsUpdate |= update(&stab.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_KP], value);
				break;
			case TXPIDSETTINGS_PIDS_YAWRATEKI:
				stabilizationSettingsNeedsUpdate |= update(&stab.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_KI], value);
				break;
			case TXPIDSETTINGS_PIDS_YAWRATEKD:
				stabilizationSettingsNeedsUpdate |= update(&stab.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_KD], value);
				break;
			case TXPIDSETTINGS_PIDS_YAWRATEILIMIT:
				stabilizationSettingsNeedsUpdate |= update(&stab.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_ILIMIT], value);
				break;
			case TXPIDSETTINGS_PIDS_YAWATTITUDEKP:
				stabilizationSettingsNeedsUpdate |= update(&stab.YawPI[STABILIZATIONSETTINGS_YAWPI_KP], value);
				break;
			case TXPIDSETTINGS_PIDS_YAWATTITUDEKI:
				stabilizationSettingsNeedsUpdate |= update(&stab.YawPI[STABILIZATIONSETTINGS_YAWPI_KI], value);
				break;
			case TXPIDSETTINGS_PIDS_YAWATTITUDEILIMIT:
				stabilizationSettingsNeedsUpdate |= update(&stab.YawPI[STABILIZATIONSETTINGS_YAWPI_ILIMIT], value);
				break;
			case TXPIDSETTINGS_PIDS_GYROCUTOFF:
				stabilizationSettingsNeedsUpdate |= update(&stab.GyroCutoff, value);
				break;
			case TXPIDSETTINGS_PIDS_ROLLVBARSENSITIVITY:
				stabilizationSettingsNeedsUpdate |= update(&stab.VbarSensitivity[STABILIZATIONSETTINGS_VBARSENSITIVITY_ROLL], value);
				break;
			case TXPIDSETTINGS_PIDS_PITCHVBARSENSITIVITY:
				stabilizationSettingsNeedsUpdate |= update(&stab.VbarSensitivity[STABILIZATIONSETTINGS_VBARSENSITIVITY_PITCH], value);
				break;
			case TXPIDSETTINGS_PIDS_ROLLPITCHVBARSENSITIVITY:
				stabilizationSettingsNeedsUpdate |= update(&stab.VbarSensitivity[STABILIZATIONSETTINGS_VBARSENSITIVITY_ROLL], value);
				stabilizationSettingsNeedsUpdate |= update(&stab.VbarSensitivity[STABILIZATIONSETTINGS_VBARSENSITIVITY_PITCH], value);
				break;
			
			case TXPIDSETTINGS_PIDS_YAWVBARSENSITIVITY:
				stabilizationSettingsNeedsUpdate |= update(&stab.VbarSensitivity[STABILIZATIONSETTINGS_VBARSENSITIVITY_YAW], value);
				break;
			case TXPIDSETTINGS_PIDS_ROLLVBARKP:
				stabilizationSettingsNeedsUpdate |= update(&stab.VbarRollPID[STABILIZATIONSETTINGS_VBARROLLPID_KP], value);
				break;
			case TXPIDSETTINGS_PIDS_ROLLVBARKI:
				stabilizationSettingsNeedsUpdate |= update(&stab.VbarRollPID[STABILIZATIONSETTINGS_VBARROLLPID_KI], value);
				break;	
			case TXPIDSETTINGS_PIDS_ROLLVBARKD:
				stabilizationSettingsNeedsUpdate |= update(&stab.VbarRollPID[STABILIZATIONSETTINGS_VBARROLLPID_KD], value);
				break;
			case TXPIDSETTINGS_PIDS_PITCHVBARKP:
				stabilizationSettingsNeedsUpdate |= update(&stab.VbarPitchPID[STABILIZATIONSETTINGS_VBARPITCHPID_KP], value);
				break;
			case TXPIDSETTINGS_PIDS_PITCHVBARKI:
				stabilizationSettingsNeedsUpdate |= update(&stab.VbarPitchPID[STABILIZATIONSETTINGS_VBARPITCHPID_KI], value);
				break;
			case TXPIDSETTINGS_PIDS_PITCHVBARKD:
				stabilizationSettingsNeedsUpdate |= update(&stab.VbarPitchPID[STABILIZATIONSETTINGS_VBARPITCHPID_KD], value);
				break;
			case TXPIDSETTINGS_PIDS_ROLLPITCHVBARKP:
				stabilizationSettingsNeedsUpdate |= update(&stab.VbarRollPID[STABILIZATIONSETTINGS_VBARROLLPID_KP], value);
				stabilizationSettingsNeedsUpdate |= update(&stab.VbarPitchPID[STABILIZATIONSETTINGS_VBARPITCHPID_KP], value);
				break;
			case TXPIDSETTINGS_PIDS_ROLLPITCHVBARKI:
				stabilizationSettingsNeedsUpdate |= update(&stab.VbarRollPID[STABILIZATIONSETTINGS_VBARROLLPID_KI], value);
				stabilizationSettingsNeedsUpdate |= update(&stab.VbarPitchPID[STABILIZATIONSETTINGS_VBARPITCHPID_KI], value);
				break;
			case TXPIDSETTINGS_PIDS_ROLLPITCHVBARKD:
				stabilizationSettingsNeedsUpdate |= update(&stab.VbarRollPID[STABILIZATIONSETTINGS_VBARROLLPID_KD], value);
				stabilizationSettingsNeedsUpdate |= update(&stab.VbarPitchPID[STABILIZATIONSETTINGS_VBARPITCHPID_KD], value);
				break;
			case TXPIDSETTINGS_PIDS_YAWVBARKP:
				stabilizationSettingsNeedsUpdate |= update(&stab.VbarYawPID[STABILIZATIONSETTINGS_VBARYAWPID_KP], value);
				break;
			case TXPIDSETTINGS_PIDS_YAWVBARKI:
				stabilizationSettingsNeedsUpdate |= update(&stab.VbarYawPID[STABILIZATIONSETTINGS_VBARYAWPID_KI], value);
				break;
			case TXPIDSETTINGS_PIDS_YAWVBARKD:
				stabilizationSettingsNeedsUpdate |= update(&stab.VbarYawPID[STABILIZATIONSETTINGS_VBARYAWPID_KD], value);
				break;
#ifdef UAVOBJ_INIT_vtolpathfollowersettings
			case TXPIDSETTINGS_PIDS_HORIZONTALPOSKP:
				vtolPathFollowerSettingsNeedsUpdate |= update(&vtolPathFollowerSettingsData.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KP], value);
				break;
			case TXPIDSETTINGS_PIDS_HORIZONTALPOSKI:
				vtolPathFollowerSettingsNeedsUpdate |= update(&vtolPathFollowerSettingsData.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KI], value);
				break;
			case TXPIDSETTINGS_PIDS_HORIZONTALPOSILIMIT:
				vtolPathFollowerSettingsNeedsUpdate |= update(&vtolPathFollowerSettingsData.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_ILIMIT], value);
				break;
			case TXPIDSETTINGS_PIDS_HORIZONTALVELKP:
				vtolPathFollowerSettingsNeedsUpdate |= update(&vtolPathFollowerSettingsData.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KP], value);
				break;
			case TXPIDSETTINGS_PIDS_HORIZONTALVELKI:
				vtolPathFollowerSettingsNeedsUpdate |= update(&vtolPathFollowerSettingsData.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KI], value);
				break;
			case TXPIDSETTINGS_PIDS_HORIZONTALVELKD:
				vtolPathFollowerSettingsNeedsUpdate |= update(&vtolPathFollowerSettingsData.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KD], value);
				break;
#endif /* UAVOBJ_INIT_vtolpathfollowersettings */
			default:
				// Previously this would assert.  But now the
				// object may be missing and it's not worth a
				// crash.
				break;
			}
		}
	}

	// Update UAVOs, if necessary
	if (stabilizationSettingsNeedsUpdate) {
		StabilizationSettingsSet(&stab);
	}

#ifdef UAVOBJ_INIT_vtolpathfollowersettings
	if (vtolPathFollowerSettingsNeedsUpdate) {
		// Check to make sure the settings UAVObject has been instantiated
		if (VtolPathFollowerSettingsHandle()) {
			VtolPathFollowerSettingsSet(&vtolPathFollowerSettingsData);
		}
	}
#endif
}

/**
 * Scales input val from [inMin..inMax] range to [outMin..outMax].
 * If val is out of input range (inMin <= inMax), it will be bound.
 * (outMin > outMax) is ok, in that case output will be decreasing.
 *
 * \returns scaled value
 */
static float scale(float val, float inMin, float inMax, float outMin, float outMax)
{
	// bound input value
	if (val > inMax) val = inMax;
	if (val < inMin) val = inMin;

	// normalize input value to [0..1]
	if (inMax <= inMin)
		val = 0.0;
	else
		val = (val - inMin) / (inMax - inMin);

	// update output bounds
	if (outMin > outMax) {
		float t = outMin;
		outMin = outMax;
		outMax = t;
		val = 1.0f - val;
	}

	return (outMax - outMin) * val + outMin;
}

/**
 * Updates var using val if needed.
 * \returns 1 if updated, 0 otherwise
 */
static bool update(float *var, float val)
{
	if (*var != val) {
		*var = val;
		return true;
	}
	return false;
}
Ejemplo n.º 3
0
/**
 * Initialise the module, called on startup
 * \returns 0 on success or -1 if initialisation failed
 */
int32_t TxPIDInitialize(void)
{
    bool txPIDEnabled;
    HwSettingsOptionalModulesData optionalModules;

#ifdef REVOLUTION
    AltitudeHoldSettingsInitialize();
#endif

    HwSettingsInitialize();
    HwSettingsOptionalModulesGet(&optionalModules);

    if (optionalModules.TxPID == HWSETTINGS_OPTIONALMODULES_ENABLED) {
        txPIDEnabled = true;
    } else {
        txPIDEnabled = false;
    }

    if (txPIDEnabled) {
        TxPIDSettingsInitialize();
        TxPIDStatusInitialize();
        AccessoryDesiredInitialize();

        UAVObjEvent ev = {
            .obj    = AccessoryDesiredHandle(),
            .instId = 0,
            .event  = 0,
            .lowPriority = false,
        };
        EventPeriodicCallbackCreate(&ev, updatePIDs, SAMPLE_PERIOD_MS / portTICK_RATE_MS);

#if (TELEMETRY_UPDATE_PERIOD_MS != 0)
        // Change StabilizationSettings update rate from OnChange to periodic
        // to prevent telemetry link flooding with frequent updates in case of
        // control channel jitter.
        // Warning: saving to flash with this code active will change the
        // StabilizationSettings update rate permanently. Use Metadata via
        // browser to reset to defaults (telemetryAcked=true, OnChange).
        UAVObjMetadata metadata;
        StabilizationSettingsInitialize();
        StabilizationSettingsGetMetadata(&metadata);
        metadata.telemetryAcked = 0;
        metadata.telemetryUpdateMode   = UPDATEMODE_PERIODIC;
        metadata.telemetryUpdatePeriod = TELEMETRY_UPDATE_PERIOD_MS;
        StabilizationSettingsSetMetadata(&metadata);

        AttitudeSettingsInitialize();
        AttitudeSettingsGetMetadata(&metadata);
        metadata.telemetryAcked = 0;
        metadata.telemetryUpdateMode   = UPDATEMODE_PERIODIC;
        metadata.telemetryUpdatePeriod = TELEMETRY_UPDATE_PERIOD_MS;
        AttitudeSettingsSetMetadata(&metadata);
#endif /* if (TELEMETRY_UPDATE_PERIOD_MS != 0) */

        return 0;
    }

    return -1;
}

/* stub: module has no module thread */
int32_t TxPIDStart(void)
{
    return 0;
}

MODULE_INITCALL(TxPIDInitialize, TxPIDStart);

/**
 * Update PIDs callback function
 */
static void updatePIDs(UAVObjEvent *ev)
{
    if (ev->obj != AccessoryDesiredHandle()) {
        return;
    }

    TxPIDSettingsData inst;
    TxPIDSettingsGet(&inst);

    if (inst.UpdateMode == TXPIDSETTINGS_UPDATEMODE_NEVER) {
        return;
    }

    uint8_t armed;
    FlightStatusArmedGet(&armed);
    if ((inst.UpdateMode == TXPIDSETTINGS_UPDATEMODE_WHENARMED) &&
        (armed == FLIGHTSTATUS_ARMED_DISARMED)) {
        return;
    }

    StabilizationBankData bank;
    switch (inst.BankNumber) {
    case 0:
        StabilizationSettingsBank1Get((StabilizationSettingsBank1Data *)&bank);
        break;

    case 1:
        StabilizationSettingsBank2Get((StabilizationSettingsBank2Data *)&bank);
        break;

    case 2:
        StabilizationSettingsBank3Get((StabilizationSettingsBank3Data *)&bank);
        break;

    default:
        return;
    }
    StabilizationSettingsData stab;
    StabilizationSettingsGet(&stab);

    AttitudeSettingsData att;
    AttitudeSettingsGet(&att);

#ifdef REVOLUTION
    AltitudeHoldSettingsData altitude;
    AltitudeHoldSettingsGet(&altitude);
#endif
    AccessoryDesiredData accessory;

    TxPIDStatusData txpid_status;
    TxPIDStatusGet(&txpid_status);

    bool easyTuneEnabled        = false;

    uint8_t needsUpdateBank     = 0;
    uint8_t needsUpdateStab     = 0;
    uint8_t needsUpdateAtt      = 0;
#ifdef REVOLUTION
    uint8_t needsUpdateAltitude = 0;
#endif

    // Loop through every enabled instance
    for (uint8_t i = 0; i < TXPIDSETTINGS_PIDS_NUMELEM; i++) {
        if (TxPIDSettingsPIDsToArray(inst.PIDs)[i] != TXPIDSETTINGS_PIDS_DISABLED) {
            float value;
            if (TxPIDSettingsInputsToArray(inst.Inputs)[i] == TXPIDSETTINGS_INPUTS_THROTTLE) {
                ManualControlCommandThrottleGet(&value);
                value = scale(value,
                              inst.ThrottleRange.Min,
                              inst.ThrottleRange.Max,
                              TxPIDSettingsMinPIDToArray(inst.MinPID)[i],
                              TxPIDSettingsMaxPIDToArray(inst.MaxPID)[i]);
            } else if (AccessoryDesiredInstGet(
                           TxPIDSettingsInputsToArray(inst.Inputs)[i] - TXPIDSETTINGS_INPUTS_ACCESSORY0,
                           &accessory) == 0) {
                value = scale(accessory.AccessoryVal, -1.0f, 1.0f,
                              TxPIDSettingsMinPIDToArray(inst.MinPID)[i],
                              TxPIDSettingsMaxPIDToArray(inst.MaxPID)[i]);
            } else {
                continue;
            }

            TxPIDStatusCurPIDToArray(txpid_status.CurPID)[i] = value;

            switch (TxPIDSettingsPIDsToArray(inst.PIDs)[i]) {
            case TXPIDSETTINGS_PIDS_ROLLRATEKP:
                needsUpdateBank |= update(&bank.RollRatePID.Kp, value);
                break;
            case TXPIDSETTINGS_PIDS_EASYTUNERATEROLL:
                easyTuneEnabled  = true;
                needsUpdateBank |= update(&bank.RollRatePID.Kp, value);
                needsUpdateBank |= update(&bank.RollRatePID.Ki, value * inst.EasyTunePitchRollRateFactors.I);
                needsUpdateBank |= update(&bank.RollRatePID.Kd, value * inst.EasyTunePitchRollRateFactors.D);
                break;
            case TXPIDSETTINGS_PIDS_EASYTUNERATEPITCH:
                easyTuneEnabled  = true;
                needsUpdateBank |= update(&bank.PitchRatePID.Kp, value);
                needsUpdateBank |= update(&bank.PitchRatePID.Ki, value * inst.EasyTunePitchRollRateFactors.I);
                needsUpdateBank |= update(&bank.PitchRatePID.Kd, value * inst.EasyTunePitchRollRateFactors.D);
                break;
            case TXPIDSETTINGS_PIDS_ROLLRATEKI:
                needsUpdateBank |= update(&bank.RollRatePID.Ki, value);
                break;
            case TXPIDSETTINGS_PIDS_ROLLRATEKD:
                needsUpdateBank |= update(&bank.RollRatePID.Kd, value);
                break;
            case TXPIDSETTINGS_PIDS_ROLLRATEILIMIT:
                needsUpdateBank |= update(&bank.RollRatePID.ILimit, value);
                break;
            case TXPIDSETTINGS_PIDS_ROLLRATERESP:
                needsUpdateBank |= updateUint16(&bank.ManualRate.Roll, value);
                break;
            case TXPIDSETTINGS_PIDS_ROLLATTITUDEKP:
                needsUpdateBank |= update(&bank.RollPI.Kp, value);
                break;
            case TXPIDSETTINGS_PIDS_ROLLATTITUDEKI:
                needsUpdateBank |= update(&bank.RollPI.Ki, value);
                break;
            case TXPIDSETTINGS_PIDS_ROLLATTITUDEILIMIT:
                needsUpdateBank |= update(&bank.RollPI.ILimit, value);
                break;
            case TXPIDSETTINGS_PIDS_ROLLATTITUDERESP:
                needsUpdateBank |= updateUint8(&bank.RollMax, value);
                break;
            case TXPIDSETTINGS_PIDS_PITCHRATEKP:
                needsUpdateBank |= update(&bank.PitchRatePID.Kp, value);
                break;
            case TXPIDSETTINGS_PIDS_PITCHRATEKI:
                needsUpdateBank |= update(&bank.PitchRatePID.Ki, value);
                break;
            case TXPIDSETTINGS_PIDS_PITCHRATEKD:
                needsUpdateBank |= update(&bank.PitchRatePID.Kd, value);
                break;
            case TXPIDSETTINGS_PIDS_PITCHRATEILIMIT:
                needsUpdateBank |= update(&bank.PitchRatePID.ILimit, value);
                break;
            case TXPIDSETTINGS_PIDS_PITCHRATERESP:
                needsUpdateBank |= updateUint16(&bank.ManualRate.Pitch, value);
                break;
            case TXPIDSETTINGS_PIDS_PITCHATTITUDEKP:
                needsUpdateBank |= update(&bank.PitchPI.Kp, value);
                break;
            case TXPIDSETTINGS_PIDS_PITCHATTITUDEKI:
                needsUpdateBank |= update(&bank.PitchPI.Ki, value);
                break;
            case TXPIDSETTINGS_PIDS_PITCHATTITUDEILIMIT:
                needsUpdateBank |= update(&bank.PitchPI.ILimit, value);
                break;
            case TXPIDSETTINGS_PIDS_PITCHATTITUDERESP:
                needsUpdateBank |= updateUint8(&bank.PitchMax, value);
                break;
            case TXPIDSETTINGS_PIDS_ROLLPITCHRATEKP:
                needsUpdateBank |= update(&bank.RollRatePID.Kp, value);
                needsUpdateBank |= update(&bank.PitchRatePID.Kp, value);
                break;
            case TXPIDSETTINGS_PIDS_ROLLPITCHRATEKI:
                needsUpdateBank |= update(&bank.RollRatePID.Ki, value);
                needsUpdateBank |= update(&bank.PitchRatePID.Ki, value);
                break;
            case TXPIDSETTINGS_PIDS_ROLLPITCHRATEKD:
                needsUpdateBank |= update(&bank.RollRatePID.Kd, value);
                needsUpdateBank |= update(&bank.PitchRatePID.Kd, value);
                break;
            case TXPIDSETTINGS_PIDS_ROLLPITCHRATEILIMIT:
                needsUpdateBank |= update(&bank.RollRatePID.ILimit, value);
                needsUpdateBank |= update(&bank.PitchRatePID.ILimit, value);
                break;
            case TXPIDSETTINGS_PIDS_ROLLPITCHRATERESP:
                needsUpdateBank |= updateUint16(&bank.ManualRate.Roll, value);
                needsUpdateBank |= updateUint16(&bank.ManualRate.Pitch, value);
                break;
            case TXPIDSETTINGS_PIDS_ROLLPITCHATTITUDEKP:
                needsUpdateBank |= update(&bank.RollPI.Kp, value);
                needsUpdateBank |= update(&bank.PitchPI.Kp, value);
                break;
            case TXPIDSETTINGS_PIDS_ROLLPITCHATTITUDEKI:
                needsUpdateBank |= update(&bank.RollPI.Ki, value);
                needsUpdateBank |= update(&bank.PitchPI.Ki, value);
                break;
            case TXPIDSETTINGS_PIDS_ROLLPITCHATTITUDEILIMIT:
                needsUpdateBank |= update(&bank.RollPI.ILimit, value);
                needsUpdateBank |= update(&bank.PitchPI.ILimit, value);
                break;
            case TXPIDSETTINGS_PIDS_ROLLPITCHATTITUDERESP:
                needsUpdateBank |= updateUint8(&bank.RollMax, value);
                needsUpdateBank |= updateUint8(&bank.PitchMax, value);
                break;
            case TXPIDSETTINGS_PIDS_YAWRATEKP:
                needsUpdateBank |= update(&bank.YawRatePID.Kp, value);
                break;
            case TXPIDSETTINGS_PIDS_YAWRATEKI:
                needsUpdateBank |= update(&bank.YawRatePID.Ki, value);
                break;
            case TXPIDSETTINGS_PIDS_YAWRATEKD:
                needsUpdateBank |= update(&bank.YawRatePID.Kd, value);
                break;
            case TXPIDSETTINGS_PIDS_YAWRATEILIMIT:
                needsUpdateBank |= update(&bank.YawRatePID.ILimit, value);
                break;
            case TXPIDSETTINGS_PIDS_YAWRATERESP:
                needsUpdateBank |= updateUint16(&bank.ManualRate.Yaw, value);
                break;
            case TXPIDSETTINGS_PIDS_YAWATTITUDEKP:
                needsUpdateBank |= update(&bank.YawPI.Kp, value);
                break;
            case TXPIDSETTINGS_PIDS_YAWATTITUDEKI:
                needsUpdateBank |= update(&bank.YawPI.Ki, value);
                break;
            case TXPIDSETTINGS_PIDS_YAWATTITUDEILIMIT:
                needsUpdateBank |= update(&bank.YawPI.ILimit, value);
                break;
            case TXPIDSETTINGS_PIDS_YAWATTITUDERESP:
                needsUpdateBank |= updateUint8(&bank.YawMax, value);
                break;
            case TXPIDSETTINGS_PIDS_ROLLEXPO:
                needsUpdateBank |= updateInt8(&bank.StickExpo.Roll, value);
                break;
            case TXPIDSETTINGS_PIDS_PITCHEXPO:
                needsUpdateBank |= updateInt8(&bank.StickExpo.Pitch, value);
                break;
            case TXPIDSETTINGS_PIDS_ROLLPITCHEXPO:
                needsUpdateBank |= updateInt8(&bank.StickExpo.Roll, value);
                needsUpdateBank |= updateInt8(&bank.StickExpo.Pitch, value);
                break;
            case TXPIDSETTINGS_PIDS_YAWEXPO:
                needsUpdateBank |= updateInt8(&bank.StickExpo.Yaw, value);
                break;
            case TXPIDSETTINGS_PIDS_GYROTAU:
                needsUpdateStab |= update(&stab.GyroTau, value);
                break;
            case TXPIDSETTINGS_PIDS_ACROROLLFACTOR:
                needsUpdateBank |= updateUint8(&bank.AcroInsanityFactor.Roll, value);
                break;
            case TXPIDSETTINGS_PIDS_ACROPITCHFACTOR:
                needsUpdateBank |= updateUint8(&bank.AcroInsanityFactor.Pitch, value);
                break;
            case TXPIDSETTINGS_PIDS_ACROROLLPITCHFACTOR:
                needsUpdateBank |= updateUint8(&bank.AcroInsanityFactor.Roll, value);
                needsUpdateBank |= updateUint8(&bank.AcroInsanityFactor.Pitch, value);
                break;
            case TXPIDSETTINGS_PIDS_ACCELTAU:
                needsUpdateAtt  |= update(&att.AccelTau, value);
                break;
            case TXPIDSETTINGS_PIDS_ACCELKP:
                needsUpdateAtt  |= update(&att.AccelKp, value);
                break;
            case TXPIDSETTINGS_PIDS_ACCELKI:
                needsUpdateAtt  |= update(&att.AccelKi, value);
                break;

#ifdef REVOLUTION
            case TXPIDSETTINGS_PIDS_ALTITUDEPOSKP:
                needsUpdateAltitude |= update(&altitude.VerticalPosP, value);
                break;
            case TXPIDSETTINGS_PIDS_ALTITUDEVELOCITYKP:
                needsUpdateAltitude |= update(&altitude.VerticalVelPID.Kp, value);
                break;
            case TXPIDSETTINGS_PIDS_ALTITUDEVELOCITYKI:
                needsUpdateAltitude |= update(&altitude.VerticalVelPID.Ki, value);
                break;
            case TXPIDSETTINGS_PIDS_ALTITUDEVELOCITYKD:
                needsUpdateAltitude |= update(&altitude.VerticalVelPID.Kd, value);
                break;
            case TXPIDSETTINGS_PIDS_ALTITUDEVELOCITYBETA:
                needsUpdateAltitude |= update(&altitude.VerticalVelPID.Beta, value);
                break;
#endif
            default:
                PIOS_Assert(0);
            }
        }
    }
    if (needsUpdateStab) {
        StabilizationSettingsSet(&stab);
    }
    if (needsUpdateAtt) {
        AttitudeSettingsSet(&att);
    }
#ifdef REVOLUTION
    if (needsUpdateAltitude) {
        AltitudeHoldSettingsSet(&altitude);
    }
#endif
    if (easyTuneEnabled && (inst.EasyTuneRatePIDRecalculateYaw != TXPIDSETTINGS_EASYTUNERATEPIDRECALCULATEYAW_FALSE)) {
        float newKp = (bank.RollRatePID.Kp + bank.PitchRatePID.Kp) * .5f * inst.EasyTuneYawRateFactors.P;
        needsUpdateBank |= update(&bank.YawRatePID.Kp, newKp);
        needsUpdateBank |= update(&bank.YawRatePID.Ki, newKp * inst.EasyTuneYawRateFactors.I);
        needsUpdateBank |= update(&bank.YawRatePID.Kd, newKp * inst.EasyTuneYawRateFactors.D);
    }
    if (needsUpdateBank) {
        switch (inst.BankNumber) {
        case 0:
            StabilizationSettingsBank1Set((StabilizationSettingsBank1Data *)&bank);
            break;

        case 1:
            StabilizationSettingsBank2Set((StabilizationSettingsBank2Data *)&bank);
            break;

        case 2:
            StabilizationSettingsBank3Set((StabilizationSettingsBank3Data *)&bank);
            break;

        default:
            return;
        }
    }

    if (needsUpdateStab ||
        needsUpdateAtt ||
#ifdef REVOLUTION
        needsUpdateAltitude ||
#endif /* REVOLUTION */
        needsUpdateBank) {
        TxPIDStatusSet(&txpid_status);;
    }
}

/**
 * Scales input val from [inMin..inMax] range to [outMin..outMax].
 * If val is out of input range (inMin <= inMax), it will be bound.
 * (outMin > outMax) is ok, in that case output will be decreasing.
 *
 * \returns scaled value
 */
static float scale(float val, float inMin, float inMax, float outMin, float outMax)
{
    // bound input value
    if (val > inMax) {
        val = inMax;
    }
    if (val < inMin) {
        val = inMin;
    }

    // normalize input value to [0..1]
    if (inMax <= inMin) {
        val = 0.0f;
    } else {
        val = (val - inMin) / (inMax - inMin);
    }

    // update output bounds
    if (outMin > outMax) {
        float t = outMin;
        outMin = outMax;
        outMax = t;
        val    = 1.0f - val;
    }

    return (outMax - outMin) * val + outMin;
}

/**
 * Updates var using val if needed.
 * \returns 1 if updated, 0 otherwise
 */
static uint8_t update(float *var, float val)
{
    /* FIXME: this is not an entirely correct way
     * to check if the two floating point
     * numbers are 'not equal'.
     * Epsilon of 1e-9 is probably okay for the range
     * of numbers we see here*/
    if (fabsf(*var - val) > 1e-9f) {
        *var = val;
        return 1;
    }
    return 0;
}

/**
 * Updates var using val if needed.
 * \returns 1 if updated, 0 otherwise
 */
static uint8_t updateUint16(uint16_t *var, float val)
{
    uint16_t roundedVal = (uint16_t)roundf(val);

    if (*var != roundedVal) {
        *var = roundedVal;
        return 1;
    }
    return 0;
}
Ejemplo n.º 4
0
/**
 * Initialise the module, called on startup
 * \returns 0 on success or -1 if initialisation failed
 */
int32_t TxPIDInitialize(void)
{
    bool txPIDEnabled;
    HwSettingsOptionalModulesData optionalModules;

    HwSettingsInitialize();
    HwSettingsOptionalModulesGet(&optionalModules);

    if (optionalModules.TxPID == HWSETTINGS_OPTIONALMODULES_ENABLED) {
        txPIDEnabled = true;
    } else {
        txPIDEnabled = false;
    }

    if (txPIDEnabled) {
        TxPIDSettingsInitialize();
        AccessoryDesiredInitialize();

        UAVObjEvent ev = {
            .obj    = AccessoryDesiredHandle(),
            .instId = 0,
            .event  = 0,
            .lowPriority = false,
        };
        EventPeriodicCallbackCreate(&ev, updatePIDs, SAMPLE_PERIOD_MS / portTICK_RATE_MS);

#if (TELEMETRY_UPDATE_PERIOD_MS != 0)
        // Change StabilizationSettings update rate from OnChange to periodic
        // to prevent telemetry link flooding with frequent updates in case of
        // control channel jitter.
        // Warning: saving to flash with this code active will change the
        // StabilizationSettings update rate permanently. Use Metadata via
        // browser to reset to defaults (telemetryAcked=true, OnChange).
        UAVObjMetadata metadata;
        StabilizationSettingsInitialize();
        StabilizationSettingsGetMetadata(&metadata);
        metadata.telemetryAcked = 0;
        metadata.telemetryUpdateMode   = UPDATEMODE_PERIODIC;
        metadata.telemetryUpdatePeriod = TELEMETRY_UPDATE_PERIOD_MS;
        StabilizationSettingsSetMetadata(&metadata);
#endif

        return 0;
    }

    return -1;
}

/* stub: module has no module thread */
int32_t TxPIDStart(void)
{
    return 0;
}

MODULE_INITCALL(TxPIDInitialize, TxPIDStart);

/**
 * Update PIDs callback function
 */
static void updatePIDs(UAVObjEvent *ev)
{
    if (ev->obj != AccessoryDesiredHandle()) {
        return;
    }

    TxPIDSettingsData inst;
    TxPIDSettingsGet(&inst);

    if (inst.UpdateMode == TXPIDSETTINGS_UPDATEMODE_NEVER) {
        return;
    }

    uint8_t armed;
    FlightStatusArmedGet(&armed);
    if ((inst.UpdateMode == TXPIDSETTINGS_UPDATEMODE_WHENARMED) &&
        (armed == FLIGHTSTATUS_ARMED_DISARMED)) {
        return;
    }

    StabilizationBankData bank;
    switch (inst.BankNumber) {
    case 0:
        StabilizationSettingsBank1Get((StabilizationSettingsBank1Data *)&bank);
        break;

    case 1:
        StabilizationSettingsBank2Get((StabilizationSettingsBank2Data *)&bank);
        break;

    case 2:
        StabilizationSettingsBank2Get((StabilizationSettingsBank2Data *)&bank);
        break;

    default:
        return;
    }
    StabilizationSettingsData stab;
    StabilizationSettingsGet(&stab);
    AccessoryDesiredData accessory;

    uint8_t needsUpdateBank = 0;
    uint8_t needsUpdateStab = 0;

    // Loop through every enabled instance
    for (uint8_t i = 0; i < TXPIDSETTINGS_PIDS_NUMELEM; i++) {
        if (cast_struct_to_array(inst.PIDs, inst.PIDs.Instance1)[i] != TXPIDSETTINGS_PIDS_DISABLED) {
            float value;
            if (cast_struct_to_array(inst.Inputs, inst.Inputs.Instance1)[i] == TXPIDSETTINGS_INPUTS_THROTTLE) {
                ManualControlCommandThrottleGet(&value);
                value = scale(value,
                              inst.ThrottleRange.Min,
                              inst.ThrottleRange.Max,
                              cast_struct_to_array(inst.MinPID, inst.MinPID.Instance1)[i],
                              cast_struct_to_array(inst.MaxPID, inst.MaxPID.Instance1)[i]);
            } else if (AccessoryDesiredInstGet(
                           cast_struct_to_array(inst.Inputs, inst.Inputs.Instance1)[i] - TXPIDSETTINGS_INPUTS_ACCESSORY0,
                           &accessory) == 0) {
                value = scale(accessory.AccessoryVal, -1.0f, 1.0f,
                              cast_struct_to_array(inst.MinPID, inst.MinPID.Instance1)[i],
                              cast_struct_to_array(inst.MaxPID, inst.MaxPID.Instance1)[i]);
            } else {
                continue;
            }

            switch (cast_struct_to_array(inst.PIDs, inst.PIDs.Instance1)[i]) {
            case TXPIDSETTINGS_PIDS_ROLLRATEKP:
                needsUpdateBank |= update(&bank.RollRatePID.Kp, value);
                break;
            case TXPIDSETTINGS_PIDS_ROLLRATEKI:
                needsUpdateBank |= update(&bank.RollRatePID.Ki, value);
                break;
            case TXPIDSETTINGS_PIDS_ROLLRATEKD:
                needsUpdateBank |= update(&bank.RollRatePID.Kd, value);
                break;
            case TXPIDSETTINGS_PIDS_ROLLRATEILIMIT:
                needsUpdateBank |= update(&bank.RollRatePID.ILimit, value);
                break;
            case TXPIDSETTINGS_PIDS_ROLLATTITUDEKP:
                needsUpdateBank |= update(&bank.RollPI.Kp, value);
                break;
            case TXPIDSETTINGS_PIDS_ROLLATTITUDEKI:
                needsUpdateBank |= update(&bank.RollPI.Ki, value);
                break;
            case TXPIDSETTINGS_PIDS_ROLLATTITUDEILIMIT:
                needsUpdateBank |= update(&bank.RollPI.ILimit, value);
                break;
            case TXPIDSETTINGS_PIDS_PITCHRATEKP:
                needsUpdateBank |= update(&bank.PitchRatePID.Kp, value);
                break;
            case TXPIDSETTINGS_PIDS_PITCHRATEKI:
                needsUpdateBank |= update(&bank.PitchRatePID.Ki, value);
                break;
            case TXPIDSETTINGS_PIDS_PITCHRATEKD:
                needsUpdateBank |= update(&bank.PitchRatePID.Kd, value);
                break;
            case TXPIDSETTINGS_PIDS_PITCHRATEILIMIT:
                needsUpdateBank |= update(&bank.PitchRatePID.ILimit, value);
                break;
            case TXPIDSETTINGS_PIDS_PITCHATTITUDEKP:
                needsUpdateBank |= update(&bank.PitchPI.Kp, value);
                break;
            case TXPIDSETTINGS_PIDS_PITCHATTITUDEKI:
                needsUpdateBank |= update(&bank.PitchPI.Ki, value);
                break;
            case TXPIDSETTINGS_PIDS_PITCHATTITUDEILIMIT:
                needsUpdateBank |= update(&bank.PitchPI.ILimit, value);
                break;
            case TXPIDSETTINGS_PIDS_ROLLPITCHRATEKP:
                needsUpdateBank |= update(&bank.RollRatePID.Kp, value);
                needsUpdateBank |= update(&bank.PitchRatePID.Kp, value);
                break;
            case TXPIDSETTINGS_PIDS_ROLLPITCHRATEKI:
                needsUpdateBank |= update(&bank.RollRatePID.Ki, value);
                needsUpdateBank |= update(&bank.PitchRatePID.Ki, value);
                break;
            case TXPIDSETTINGS_PIDS_ROLLPITCHRATEKD:
                needsUpdateBank |= update(&bank.RollRatePID.Kd, value);
                needsUpdateBank |= update(&bank.PitchRatePID.Kd, value);
                break;
            case TXPIDSETTINGS_PIDS_ROLLPITCHRATEILIMIT:
                needsUpdateBank |= update(&bank.RollRatePID.ILimit, value);
                needsUpdateBank |= update(&bank.PitchRatePID.ILimit, value);
                break;
            case TXPIDSETTINGS_PIDS_ROLLPITCHATTITUDEKP:
                needsUpdateBank |= update(&bank.RollPI.Kp, value);
                needsUpdateBank |= update(&bank.PitchPI.Kp, value);
                break;
            case TXPIDSETTINGS_PIDS_ROLLPITCHATTITUDEKI:
                needsUpdateBank |= update(&bank.RollPI.Ki, value);
                needsUpdateBank |= update(&bank.PitchPI.Ki, value);
                break;
            case TXPIDSETTINGS_PIDS_ROLLPITCHATTITUDEILIMIT:
                needsUpdateBank |= update(&bank.RollPI.ILimit, value);
                needsUpdateBank |= update(&bank.PitchPI.ILimit, value);
                break;
            case TXPIDSETTINGS_PIDS_YAWRATEKP:
                needsUpdateBank |= update(&bank.YawRatePID.Kp, value);
                break;
            case TXPIDSETTINGS_PIDS_YAWRATEKI:
                needsUpdateBank |= update(&bank.YawRatePID.Ki, value);
                break;
            case TXPIDSETTINGS_PIDS_YAWRATEKD:
                needsUpdateBank |= update(&bank.YawRatePID.Kd, value);
                break;
            case TXPIDSETTINGS_PIDS_YAWRATEILIMIT:
                needsUpdateBank |= update(&bank.YawRatePID.ILimit, value);
                break;
            case TXPIDSETTINGS_PIDS_YAWATTITUDEKP:
                needsUpdateBank |= update(&bank.YawPI.Kp, value);
                break;
            case TXPIDSETTINGS_PIDS_YAWATTITUDEKI:
                needsUpdateBank |= update(&bank.YawPI.Ki, value);
                break;
            case TXPIDSETTINGS_PIDS_YAWATTITUDEILIMIT:
                needsUpdateBank |= update(&bank.YawPI.ILimit, value);
                break;
            case TXPIDSETTINGS_PIDS_GYROTAU:
                needsUpdateStab |= update(&stab.GyroTau, value);
                break;
            default:
                PIOS_Assert(0);
            }
        }
    }
    if (needsUpdateStab) {
        StabilizationSettingsSet(&stab);
    }
    if (needsUpdateBank) {
        switch (inst.BankNumber) {
        case 0:
            StabilizationSettingsBank1Set((StabilizationSettingsBank1Data *)&bank);
            break;

        case 1:
            StabilizationSettingsBank2Set((StabilizationSettingsBank2Data *)&bank);
            break;

        case 2:
            StabilizationSettingsBank3Set((StabilizationSettingsBank3Data *)&bank);
            break;

        default:
            return;
        }
    }
}

/**
 * Scales input val from [inMin..inMax] range to [outMin..outMax].
 * If val is out of input range (inMin <= inMax), it will be bound.
 * (outMin > outMax) is ok, in that case output will be decreasing.
 *
 * \returns scaled value
 */
static float scale(float val, float inMin, float inMax, float outMin, float outMax)
{
    // bound input value
    if (val > inMax) {
        val = inMax;
    }
    if (val < inMin) {
        val = inMin;
    }

    // normalize input value to [0..1]
    if (inMax <= inMin) {
        val = 0.0f;
    } else {
        val = (val - inMin) / (inMax - inMin);
    }

    // update output bounds
    if (outMin > outMax) {
        float t = outMin;
        outMin = outMax;
        outMax = t;
        val    = 1.0f - val;
    }

    return (outMax - outMin) * val + outMin;
}

/**
 * Updates var using val if needed.
 * \returns 1 if updated, 0 otherwise
 */
static uint8_t update(float *var, float val)
{
    /* FIXME: this is not an entirely correct way
     * to check if the two floating point
     * numbers are 'not equal'.
     * Epsilon of 1e-9 is probably okay for the range
     * of numbers we see here*/
    if (fabsf(*var - val) > 1e-9f) {
        *var = val;
        return 1;
    }
    return 0;
}