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