From a9af53b4f364fcad455e61570eca7be0f0de21f3 Mon Sep 17 00:00:00 2001 From: James Cotton Date: Tue, 31 Jul 2012 02:04:36 -0500 Subject: [PATCH] Create new autotuning module which rotates through the axes for autotuning and then computes new stabilization settings. --- flight/Modules/Autotune/autotune.c | 291 ++++++++++++++++++ flight/Modules/Autotune/inc/autotune.h | 37 +++ shared/uavobjectdefinition/flightstatus.xml | 2 +- .../manualcontrolsettings.xml | 2 +- 4 files changed, 330 insertions(+), 2 deletions(-) create mode 100644 flight/Modules/Autotune/autotune.c create mode 100644 flight/Modules/Autotune/inc/autotune.h diff --git a/flight/Modules/Autotune/autotune.c b/flight/Modules/Autotune/autotune.c new file mode 100644 index 000000000..3fc9246e4 --- /dev/null +++ b/flight/Modules/Autotune/autotune.c @@ -0,0 +1,291 @@ +/** + ****************************************************************************** + * @addtogroup OpenPilotModules OpenPilot Modules + * @{ + * @addtogroup Autotuning module + * @brief Reads from @ref ManualControlCommand and fakes a rate mode while + * toggling the channels to relay mode + * @{ + * + * @file autotune.c + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief Module to handle all comms to the AHRS on a periodic basis. + * + * @see The GNU Public License (GPL) Version 3 + * + ******************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + +/** + * Input objects: None, takes sensor data via pios + * Output objects: @ref AttitudeRaw @ref AttitudeActual + * + * This module computes an attitude estimate from the sensor data + * + * The module executes in its own thread. + * + * UAVObjects are automatically generated by the UAVObjectGenerator from + * the object definition XML file. + * + * Modules have no API, all communication to other modules is done through UAVObjects. + * However modules may use the API exposed by shared libraries. + * See the OpenPilot wiki for more details. + * http://www.openpilot.org/OpenPilot_Application_Architecture + * + */ + +#include "pios.h" +#include "flightstatus.h" +#include "manualcontrolcommand.h" +#include "manualcontrolsettings.h" +#include "relaytuning.h" +#include "relaytuningsettings.h" +#include "stabilizationdesired.h" +#include "stabilizationsettings.h" +#include + +// Private constants +#define STACK_SIZE_BYTES 1024 +#define TASK_PRIORITY (tskIDLE_PRIORITY+2) + +// Private types +enum AUTOTUNE_STATE {AT_INIT, AT_START, AT_ROLL, AT_PITCH, AT_FINISHED, AT_SET}; + +// Private variables +static xTaskHandle taskHandle; + +// Private functions +static void AutotuneTask(void *parameters); +static void update_stabilization_settings(); + +/** + * Initialise the module, called on startup + * \returns 0 on success or -1 if initialisation failed + */ +int32_t AutotuneInitialize(void) +{ + // Create a queue, connect to manual control command and flightstatus + + return 0; +} + +/** + * Initialise the module, called on startup + * \returns 0 on success or -1 if initialisation failed + */ +int32_t AutotuneStart(void) +{ + + // Start main task + xTaskCreate(AutotuneTask, (signed char *)"Autotune", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &taskHandle); + + //TaskMonitorAdd(TASKINFO_RUNNING_ATTITUDE, taskHandle); + //PIOS_WDG_RegisterFlag(PIOS_WDG_ATTITUDE); + + return 0; +} + +MODULE_INITCALL(AutotuneInitialize, AutotuneStart) + +/** + * Module thread, should not return. + */ +static void AutotuneTask(void *parameters) +{ + //AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE); + + enum AUTOTUNE_STATE state = AT_INIT; + + portTickType lastUpdateTime = xTaskGetTickCount(); + + while(1) { + // TODO: + // 1. get from queue + // 2. based on whether it is flightstatus or manualcontrol + + portTickType diffTime; + + const uint32_t PREPARE_TIME = 2000; + const uint32_t MEAURE_TIME = 10000; + + FlightStatusData flightStatus; + FlightStatusGet(&flightStatus); + + // Only allow this module to run when autotuning + if (flightStatus.FlightMode != FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE) { + state = AT_INIT; + vTaskDelay(50); + continue; + } + + StabilizationDesiredData stabDesired; + StabilizationDesiredGet(&stabDesired); + + StabilizationSettingsData stabSettings; + StabilizationSettingsGet(&stabSettings); + + ManualControlSettingsData manualSettings; + ManualControlSettingsGet(&manualSettings); + + ManualControlCommandData manualControl; + ManualControlCommandGet(&manualControl); + + stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE; + stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE; + stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE; + + stabDesired.Roll = manualControl.Roll * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_ROLL]; + stabDesired.Pitch = manualControl.Pitch * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_PITCH]; + stabDesired.Yaw = manualControl.Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW]; + stabDesired.Throttle = manualControl.Throttle; + + switch(state) { + case AT_INIT: + + lastUpdateTime = xTaskGetTickCount(); + + // Only start when armed and flying + if (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED && stabDesired.Throttle > 0) + state = AT_START; + break; + + case AT_START: + + diffTime = xTaskGetTickCount() - lastUpdateTime; + + // Spend the first block of time in normal rate mode to get airborne + if (diffTime > PREPARE_TIME) { + state = AT_ROLL; + lastUpdateTime = xTaskGetTickCount(); + } + break; + + case AT_ROLL: + + diffTime = xTaskGetTickCount() - lastUpdateTime; + + // Run relay mode on the roll axis for the measurement time + stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_RELAY; + if (diffTime > MEAURE_TIME) { // Move on to next state + state = AT_PITCH; + lastUpdateTime = xTaskGetTickCount(); + } + break; + + case AT_PITCH: + + diffTime = xTaskGetTickCount() - lastUpdateTime; + + // Run relay mode on the pitch axis for the measurement time + stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_RELAY; + if (diffTime > MEAURE_TIME) { // Move on to next state + state = AT_FINISHED; + lastUpdateTime = xTaskGetTickCount(); + } + break; + + case AT_FINISHED: + + // Wait until disarmed and landed before updating the settings + if (flightStatus.Armed == FLIGHTSTATUS_ARMED_DISARMED && stabDesired.Throttle <= 0) + state = AT_SET; + + break; + + case AT_SET: + update_stabilization_settings(); + state = AT_INIT; + break; + + default: + // Set an alarm or some shit like that + break; + } + + StabilizationDesiredSet(&stabDesired); + + vTaskDelay(10); + } +} + +/** + * 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; + + float input = relaySettings.Amplitude * 4.0f / M_PI; // amplitude of input (fundamental component of fourier series for the square wave) + + // For now just run over roll and pitch + for (uint i = 0; i < 2; i++) { + float output = relayTuning.Amplitude[i]; // amplitude of sinusoidal oscillation in output + float wu = 1000.0f * 2 * M_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 = input / output; // 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; + } + } + StabilizationSettingsSet(&stabSettings); +} + +/** + * @} + * @} + */ diff --git a/flight/Modules/Autotune/inc/autotune.h b/flight/Modules/Autotune/inc/autotune.h new file mode 100644 index 000000000..3f90e56a6 --- /dev/null +++ b/flight/Modules/Autotune/inc/autotune.h @@ -0,0 +1,37 @@ +/** + ****************************************************************************** + * @addtogroup OpenPilotModules OpenPilot Modules + * @{ + * @addtogroup Attitude Attitude Module + * @{ + * + * @file attitude.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2011. + * @brief Acquires sensor data and fuses it into attitude estimate for CC + * + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ +#ifndef ATTITUDE_H +#define ATTITUDE_H + +#include "openpilot.h" + +int32_t AttitudeInitialize(void); + +#endif // ATTITUDE_H diff --git a/shared/uavobjectdefinition/flightstatus.xml b/shared/uavobjectdefinition/flightstatus.xml index 22c4f9360..ba24d9bff 100644 --- a/shared/uavobjectdefinition/flightstatus.xml +++ b/shared/uavobjectdefinition/flightstatus.xml @@ -4,7 +4,7 @@ - + diff --git a/shared/uavobjectdefinition/manualcontrolsettings.xml b/shared/uavobjectdefinition/manualcontrolsettings.xml index 8a52b3b23..b9babcc4d 100644 --- a/shared/uavobjectdefinition/manualcontrolsettings.xml +++ b/shared/uavobjectdefinition/manualcontrolsettings.xml @@ -24,7 +24,7 @@ - +