示例#1
0
/**
 * Save all metaobjects to the SD card.
 * @return 0 if success or -1 if failure
 */
int32_t UAVObjSaveMetaobjects()
{
	  ObjectList *objEntry;

	  // Get lock
	  xSemaphoreTakeRecursive(mutex, portMAX_DELAY);

	  // Save all settings objects
	  LL_FOREACH(objList, objEntry) {
		    // Check if this is a settings object
		    if (objEntry->isMetaobject) {
			      // Save object
			      if (UAVObjSave((UAVObjHandle) objEntry, 0) ==
				  -1) {
					xSemaphoreGiveRecursive(mutex);
					return -1;
			      }
		    }
	  }

	  // Done
	  xSemaphoreGiveRecursive(mutex);
	  return 0;
}
int32_t writeParametersToStorage()
{
	//					// WORKING
	//					ObjectPersistenceData objper;
	//
	//					// Write all objects individually
	//					// for;;
	//					ObjectPersistenceGet(&objper);
	//
	//					ActuatorSettingsData settings;
	//					ActuatorSettingsGet(&settings);
	//
	//					objper.Selection = OBJECTPERSISTENCE_SELECTION_SINGLEOBJECT;
	//					objper.Operation = OBJECTPERSISTENCE_OPERATION_SAVE;
	//					objper.ObjectID = ACTUATORSETTINGS_OBJID;
	//					objper.InstanceID = 0;
	//					ObjectPersistenceSet(&objper);
	//					// END WORKING



	UAVObjHandle handle = ActuatorSettingsHandle();
	return UAVObjSave(handle, 0);
}
示例#3
0
/**
 * Function called in response to object updates
 */
static void objectUpdatedCb(UAVObjEvent * ev)
{
	ObjectPersistenceData objper;
	UAVObjHandle obj;

	// If the object updated was the ObjectPersistence execute requested action
	if (ev->obj == ObjectPersistenceHandle()) {
		// Get object data
		ObjectPersistenceGet(&objper);

		int retval = -1;
		// Execute action
		if (objper.Operation == OBJECTPERSISTENCE_OPERATION_LOAD) {
			if (objper.Selection == OBJECTPERSISTENCE_SELECTION_SINGLEOBJECT) {
				// Get selected object
				obj = UAVObjGetByID(objper.ObjectID);
				if (obj == 0) {
					return;
				}
				// Load selected instance
				retval = UAVObjLoad(obj, objper.InstanceID);
			} else if (objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLSETTINGS
				   || objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLOBJECTS) {
				retval = UAVObjLoadSettings();
			} else if (objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLMETAOBJECTS
				   || objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLOBJECTS) {
				retval = UAVObjLoadMetaobjects();
			}
		} else if (objper.Operation == OBJECTPERSISTENCE_OPERATION_SAVE) {
			if (objper.Selection == OBJECTPERSISTENCE_SELECTION_SINGLEOBJECT) {
				// Get selected object
				obj = UAVObjGetByID(objper.ObjectID);
				if (obj == 0) {
					return;
				}
				// Save selected instance
				retval = UAVObjSave(obj, objper.InstanceID);
			} else if (objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLSETTINGS
				   || objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLOBJECTS) {
				retval = UAVObjSaveSettings();
			} else if (objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLMETAOBJECTS
				   || objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLOBJECTS) {
				retval = UAVObjSaveMetaobjects();
			}
		} else if (objper.Operation == OBJECTPERSISTENCE_OPERATION_DELETE) {
			if (objper.Selection == OBJECTPERSISTENCE_SELECTION_SINGLEOBJECT) {
				// Get selected object
				obj = UAVObjGetByID(objper.ObjectID);
				if (obj == 0) {
					return;
				}
				// Delete selected instance
				retval = UAVObjDelete(obj, objper.InstanceID);
			} else if (objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLSETTINGS
				   || objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLOBJECTS) {
				retval = UAVObjDeleteSettings();
			} else if (objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLMETAOBJECTS
				   || objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLOBJECTS) {
				retval = UAVObjDeleteMetaobjects();
			}
		}
		if(retval == 0) { 
			objper.Operation = OBJECTPERSISTENCE_OPERATION_COMPLETED;
			ObjectPersistenceSet(&objper);
		}
	}
}
示例#4
0
/**
 * Function called in response to object updates
 */
static void objectUpdatedCb(UAVObjEvent * ev)
{
	ObjectPersistenceData objper;
	UAVObjHandle obj;

//	// DEBUG
//	if (ev->obj == SystemAlarmsHandle()) {
//		SystemAlarmsData alarm;
//		SystemAlarmsGet(&alarm);
//		PIOS_COM_SendFormattedString(PIOS_COM_DEBUG, "ALARM: %d\r\n", alarm.Alarm);
//	}

	// If the object updated was the ObjectPersistence execute requested action
	if (ev->obj == ObjectPersistenceHandle()) {
		// Get object data
		ObjectPersistenceGet(&objper);

		int retval = -1;
		// Execute action
		if (objper.Operation == OBJECTPERSISTENCE_OPERATION_LOAD) {
			if (objper.Selection == OBJECTPERSISTENCE_SELECTION_SINGLEOBJECT) {
				// Get selected object
				obj = UAVObjGetByID(objper.ObjectID);
				if (obj == 0) {
					return;
				}
				// Load selected instance
				retval = UAVObjLoad(obj, objper.InstanceID);
			} else if (objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLSETTINGS
				   || objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLOBJECTS) {
				retval = UAVObjLoadSettings();
			} else if (objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLMETAOBJECTS
				   || objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLOBJECTS) {
				retval = UAVObjLoadMetaobjects();
			}
		} else if (objper.Operation == OBJECTPERSISTENCE_OPERATION_SAVE) {
			if (objper.Selection == OBJECTPERSISTENCE_SELECTION_SINGLEOBJECT) {
				// Get selected object
				obj = UAVObjGetByID(objper.ObjectID);
				if (obj == 0) {
					return;
				}
				// Save selected instance
				retval = UAVObjSave(obj, objper.InstanceID);
			} else if (objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLSETTINGS
				   || objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLOBJECTS) {
				retval = UAVObjSaveSettings();
			} else if (objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLMETAOBJECTS
				   || objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLOBJECTS) {
				retval = UAVObjSaveMetaobjects();
			}
		} else if (objper.Operation == OBJECTPERSISTENCE_OPERATION_DELETE) {
			if (objper.Selection == OBJECTPERSISTENCE_SELECTION_SINGLEOBJECT) {
				// Get selected object
				obj = UAVObjGetByID(objper.ObjectID);
				if (obj == 0) {
					return;
				}
				// Delete selected instance
				retval = UAVObjDelete(obj, objper.InstanceID);
			} else if (objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLSETTINGS
				   || objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLOBJECTS) {
				retval = UAVObjDeleteSettings();
			} else if (objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLMETAOBJECTS
				   || objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLOBJECTS) {
				retval = UAVObjDeleteMetaobjects();
			}
		} else if (objper.Operation == OBJECTPERSISTENCE_OPERATION_FULLERASE) {
			retval = -1;
#if defined(PIOS_INCLUDE_FLASH_SECTOR_SETTINGS)
			retval = PIOS_FLASHFS_Format();
#endif
		}
		if(retval == 0) { 
			objper.Operation = OBJECTPERSISTENCE_OPERATION_COMPLETED;
			ObjectPersistenceSet(&objper);
		}
	}
}
示例#5
0
文件: autotune.c 项目: 1heinz/TauLabs
/**
 * 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;
	}
	
}
示例#6
0
/**
 * Function called in response to object updates
 */
static void objectUpdatedCb(UAVObjEvent * ev)
{
	ObjectPersistenceData objper;
	UAVObjHandle obj;

	// If the object updated was the ObjectPersistence execute requested action
	if (ev->obj == ObjectPersistenceHandle()) {
		// Get object data
		ObjectPersistenceGet(&objper);

		int retval = 1;
		
		// When this is called because of this method don't do anything
		if (objper.Operation == OBJECTPERSISTENCE_OPERATION_ERROR ||
			objper.Operation == OBJECTPERSISTENCE_OPERATION_COMPLETED) {
			return;
		}

		if (objper.Operation == OBJECTPERSISTENCE_OPERATION_LOAD) {
			if (objper.Selection == OBJECTPERSISTENCE_SELECTION_SINGLEOBJECT) {
				// Get selected object
				obj = UAVObjGetByID(objper.ObjectID);
				if (obj == 0) {
					return;
				}
				// Load selected instance
				retval = UAVObjLoad(obj, objper.InstanceID);
			} else if (objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLSETTINGS
				   || objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLOBJECTS) {
				retval = UAVObjLoadSettings();
			} else if (objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLMETAOBJECTS
				   || objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLOBJECTS) {
				retval = UAVObjLoadMetaobjects();
			}
		} else if (objper.Operation == OBJECTPERSISTENCE_OPERATION_SAVE) {
			if (objper.Selection == OBJECTPERSISTENCE_SELECTION_SINGLEOBJECT) {
				// Get selected object
				obj = UAVObjGetByID(objper.ObjectID);
				if (obj == 0) {
					return;
				}
				// Save selected instance
				retval = UAVObjSave(obj, objper.InstanceID);

				// Not sure why this is needed
				PIOS_Thread_Sleep(10);

				// Verify saving worked
				if (retval == 0)
					retval = UAVObjLoad(obj, objper.InstanceID);
			} else if (objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLSETTINGS
				   || objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLOBJECTS) {
				retval = UAVObjSaveSettings();
			} else if (objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLMETAOBJECTS
				   || objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLOBJECTS) {
				retval = UAVObjSaveMetaobjects();
			}
		} else if (objper.Operation == OBJECTPERSISTENCE_OPERATION_DELETE) {
			if (objper.Selection == OBJECTPERSISTENCE_SELECTION_SINGLEOBJECT) {
				// Delete selected instance
				retval = UAVObjDeleteById(objper.ObjectID, objper.InstanceID);
			} else if (objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLSETTINGS
				   || objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLOBJECTS) {
				retval = UAVObjDeleteSettings();
			} else if (objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLMETAOBJECTS
				   || objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLOBJECTS) {
				retval = UAVObjDeleteMetaobjects();
			}
		} else if (objper.Operation == OBJECTPERSISTENCE_OPERATION_FULLERASE) {
			retval = -1;
#if defined(PIOS_INCLUDE_LOGFS_SETTINGS)
			extern uintptr_t pios_uavo_settings_fs_id;
			retval = PIOS_FLASHFS_Format(pios_uavo_settings_fs_id);
#endif
		}
		switch(retval) {
			case 0:
				objper.Operation = OBJECTPERSISTENCE_OPERATION_COMPLETED;
				ObjectPersistenceSet(&objper);
				break;
			case -1:
				objper.Operation = OBJECTPERSISTENCE_OPERATION_ERROR;
				ObjectPersistenceSet(&objper);
				break;
			default:
				break;
		}
	}
}
示例#7
0
/**
 * Function called in response to object updates
 */
static void objectUpdatedCb(UAVObjEvent * ev)
{
	ObjectPersistenceData objper;
	UAVObjHandle obj;

	// If the object updated was the ObjectPersistence execute requested action
	if (ev->obj == ObjectPersistenceHandle()) {
		// Get object data
		ObjectPersistenceGet(&objper);

		int retval = 1;
		// Execute action
		if (objper.Operation == OBJECTPERSISTENCE_OPERATION_LOAD) {
			if (objper.Selection == OBJECTPERSISTENCE_SELECTION_SINGLEOBJECT) {
				// Get selected object
				obj = UAVObjGetByID(objper.ObjectID);
				if (obj == 0) {
					return;
				}
				// Load selected instance
				retval = UAVObjLoad(obj, objper.InstanceID);
			} else if (objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLSETTINGS
				   || objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLOBJECTS) {
				retval = UAVObjLoadSettings();
			} else if (objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLMETAOBJECTS
				   || objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLOBJECTS) {
				retval = UAVObjLoadMetaobjects();
			}
		} else if (objper.Operation == OBJECTPERSISTENCE_OPERATION_SAVE) {
			if (objper.Selection == OBJECTPERSISTENCE_SELECTION_SINGLEOBJECT) {
				// Get selected object
				obj = UAVObjGetByID(objper.ObjectID);
				if (obj == 0) {
					return;
				}
				// Save selected instance
				retval = UAVObjSave(obj, objper.InstanceID);

				// Not sure why this is needed
				vTaskDelay(10);

				// Verify saving worked
				if (retval == 0)
					retval = UAVObjLoad(obj, objper.InstanceID);
			} else if (objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLSETTINGS
				   || objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLOBJECTS) {
				retval = UAVObjSaveSettings();
			} else if (objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLMETAOBJECTS
				   || objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLOBJECTS) {
				retval = UAVObjSaveMetaobjects();
			}
		} else if (objper.Operation == OBJECTPERSISTENCE_OPERATION_DELETE) {
			if (objper.Selection == OBJECTPERSISTENCE_SELECTION_SINGLEOBJECT) {
				// Get selected object
				obj = UAVObjGetByID(objper.ObjectID);
				if (obj == 0) {
					return;
				}
				// Delete selected instance
				retval = UAVObjDelete(obj, objper.InstanceID);
			} else if (objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLSETTINGS
				   || objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLOBJECTS) {
				retval = UAVObjDeleteSettings();
			} else if (objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLMETAOBJECTS
				   || objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLOBJECTS) {
				retval = UAVObjDeleteMetaobjects();
			}
		} else if (objper.Operation == OBJECTPERSISTENCE_OPERATION_FULLERASE) {
			retval = -1;
#if defined(PIOS_INCLUDE_FLASH_SECTOR_SETTINGS)
			retval = PIOS_FLASHFS_Format();
#elif defined(PIOS_INCLUDE_FLASH_COMPACT_SETTINGS)
			retval = PIOS_FLASHFS_Compact_Format();
#endif
		}
		switch(retval) {
			case 0:
				objper.Operation = OBJECTPERSISTENCE_OPERATION_COMPLETED;
				ObjectPersistenceSet(&objper);
				break;
			case -1:
				objper.Operation = OBJECTPERSISTENCE_OPERATION_ERROR;
				ObjectPersistenceSet(&objper);
				break;
			default:
				break;
		}
	}
}
示例#8
0
/**
 * Module thread, should not return.
 */
static void AutotuneTask(void *parameters)
{
	enum AUTOTUNE_STATE state = AT_INIT;

	uint32_t last_update_time = PIOS_Thread_Systime();

	float X[AF_NUMX] = {0};
	float P[AF_NUMP] = {0};
	float noise[3] = {0};

	af_init(X,P);

	uint32_t last_time = 0.0f;
	const uint32_t YIELD_MS = 2;

	GyrosConnectCallback(at_new_gyro_data);

	while(1) {
		PIOS_WDG_UpdateFlag(PIOS_WDG_AUTOTUNE);

		uint32_t diff_time;

		const uint32_t PREPARE_TIME = 2000;
		const uint32_t MEASURE_TIME = 60000;

		static uint32_t update_counter = 0;

		bool doing_ident = false;
		bool can_sleep = true;

		FlightStatusData flightStatus;
		FlightStatusGet(&flightStatus);

		// Only allow this module to run when autotuning
		if (flightStatus.FlightMode != FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE) {
			state = AT_INIT;
			PIOS_Thread_Sleep(50);
			continue;
		}

		switch(state) {
			case AT_INIT:
				last_update_time = PIOS_Thread_Systime();

				// Only start when armed and flying
				if (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED) {

					af_init(X,P);

					UpdateSystemIdent(X, NULL, 0.0f, 0, 0);

					state = AT_START;

				}

				break;

			case AT_START:

				diff_time = PIOS_Thread_Systime() - last_update_time;

				// Spend the first block of time in normal rate mode to get airborne
				if (diff_time > PREPARE_TIME) {
					last_time = PIOS_DELAY_GetRaw();

					/* Drain the queue of all current data */
					while (circ_queue_read_pos(at_queue)) {
						circ_queue_read_completed(at_queue);
					}

					/* And reset the point spill counter */

					update_counter = 0;
					at_points_spilled = 0;

					state = AT_RUN;
					last_update_time = PIOS_Thread_Systime();
				}


				break;

			case AT_RUN:

				diff_time = PIOS_Thread_Systime() - last_update_time;

				doing_ident = true;
				can_sleep = false;

				for (int i=0; i<MAX_PTS_PER_CYCLE; i++) {
					struct at_queued_data *pt;

					/* Grab an autotune point */
					pt = circ_queue_read_pos(at_queue);

					if (!pt) {
						/* We've drained the buffer
						 * fully.  Yay! */
						can_sleep = true;
						break;
					}

					/* calculate time between successive
					 * points */

					float dT_s = PIOS_DELAY_DiffuS2(last_time,
							pt->raw_time) * 1.0e-6f;

					/* This is for the first point, but
					 * also if we have extended drops */
					if (dT_s > 0.010f) {
						dT_s = 0.010f;
					}

					last_time = pt->raw_time;

					af_predict(X, P, pt->u, pt->y, dT_s);

					// Update uavo every 256 cycles to avoid
					// telemetry spam
					if (!((update_counter++) & 0xff)) {
						UpdateSystemIdent(X, noise, dT_s, update_counter, at_points_spilled);
					}

					for (uint32_t i = 0; i < 3; i++) {
						const float NOISE_ALPHA = 0.9997f;  // 10 second time constant at 300 Hz
						noise[i] = NOISE_ALPHA * noise[i] + (1-NOISE_ALPHA) * (pt->y[i] - X[i]) * (pt->y[i] - X[i]);
					}

					/* Free the buffer containing an AT point */
					circ_queue_read_completed(at_queue);
				}

				if (diff_time > MEASURE_TIME) { // Move on to next state
					state = AT_FINISHED;
					last_update_time = PIOS_Thread_Systime();
				}

				break;

			case AT_FINISHED:

				// Wait until disarmed and landed before saving the settings

				UpdateSystemIdent(X, noise, 0, update_counter, at_points_spilled);

				state = AT_WAITING;	// Fall through

			case AT_WAITING:

				// TODO do this unconditionally on disarm,
				// no matter what mode we're in.
				if (flightStatus.Armed == FLIGHTSTATUS_ARMED_DISARMED) {
					// Save the settings locally. 
					UAVObjSave(SystemIdentHandle(), 0);
					state = AT_INIT;
				}
				break;

			default:
				// Set an alarm or some shit like that
				break;
		}

		// Update based on manual controls
		UpdateStabilizationDesired(doing_ident);

		if (can_sleep) {
			PIOS_Thread_Sleep(YIELD_MS);
		}
	}
}