From d923117c25f1306b9fc8ffc653ce21d96b50ecc6 Mon Sep 17 00:00:00 2001 From: Oleg Semyonov Date: Fri, 16 Sep 2011 23:36:44 +0300 Subject: [PATCH] TxPID: optional module to tune PID settings using R/C transmitter This module will periodically update values of stabilization PID settings depending on configured input control channels. New values of stabilization settings are not saved to flash, but updated in RAM. It is expected that the module will be enabled only for tuning. When desired values are found, they can be read via GCS and saved permanently. Then this module should be disabled again. --- flight/CopterControl/Makefile | 3 +- flight/CopterControl/System/coptercontrol.c | 4 + flight/Modules/TxPID/inc/txpid.h | 42 +++ flight/Modules/TxPID/txpid.c | 248 ++++++++++++++++++ .../src/plugins/uavobjects/uavobjects.pro | 2 + shared/uavobjectdefinition/hwsettings.xml | 2 +- shared/uavobjectdefinition/txpidsettings.xml | 25 ++ 7 files changed, 324 insertions(+), 2 deletions(-) create mode 100644 flight/Modules/TxPID/inc/txpid.h create mode 100644 flight/Modules/TxPID/txpid.c create mode 100644 shared/uavobjectdefinition/txpidsettings.xml diff --git a/flight/CopterControl/Makefile b/flight/CopterControl/Makefile index ea6cab01c..8b9b6dbcd 100644 --- a/flight/CopterControl/Makefile +++ b/flight/CopterControl/Makefile @@ -65,7 +65,7 @@ endif FLASH_TOOL = OPENOCD # List of modules to include -MODULES = Attitude Stabilization Actuator ManualControl FirmwareIAP CameraStab +MODULES = Attitude Stabilization Actuator ManualControl FirmwareIAP CameraStab TxPID # Telemetry must be last to grab the optional modules MODULES += Telemetry @@ -172,6 +172,7 @@ SRC += $(OPUAVSYNTHDIR)/receiveractivity.c SRC += $(OPUAVSYNTHDIR)/taskinfo.c SRC += $(OPUAVSYNTHDIR)/mixerstatus.c SRC += $(OPUAVSYNTHDIR)/ratedesired.c +SRC += $(OPUAVSYNTHDIR)/txpidsettings.c endif diff --git a/flight/CopterControl/System/coptercontrol.c b/flight/CopterControl/System/coptercontrol.c index 33cb096c2..ee2132671 100644 --- a/flight/CopterControl/System/coptercontrol.c +++ b/flight/CopterControl/System/coptercontrol.c @@ -37,6 +37,7 @@ #include "uavobjectsinit.h" #include "hwsettings.h" #include "camerastab.h" +#include "txpid.h" #include "systemmod.h" /* Task Priorities */ @@ -81,6 +82,9 @@ int main() if(optionalModules[HWSETTINGS_OPTIONALMODULES_CAMERASTABILIZATION] == HWSETTINGS_OPTIONALMODULES_ENABLED) { CameraStabInitialize(); } + if(optionalModules[HWSETTINGS_OPTIONALMODULES_TXPID] == HWSETTINGS_OPTIONALMODULES_ENABLED) { + TxPIDInitialize(); + } /* swap the stack to use the IRQ stack */ Stack_Change(); diff --git a/flight/Modules/TxPID/inc/txpid.h b/flight/Modules/TxPID/inc/txpid.h new file mode 100644 index 000000000..c5a2e4fa3 --- /dev/null +++ b/flight/Modules/TxPID/inc/txpid.h @@ -0,0 +1,42 @@ +/** + ****************************************************************************** + * @addtogroup OpenPilotModules OpenPilot Modules + * @{ + * @addtogroup TxPIDModule TxPID Module + * @{ + * + * @file txpid.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2011. + * @brief Optional module to tune PID settings using R/C transmitter. + * + * @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 TXPID_H +#define TXPID_H + +#include "openpilot.h" + +int32_t TxPIDInitialize(void); + +#endif // TXPID_H + +/** + * @} + * @} + */ diff --git a/flight/Modules/TxPID/txpid.c b/flight/Modules/TxPID/txpid.c new file mode 100644 index 000000000..7cf477037 --- /dev/null +++ b/flight/Modules/TxPID/txpid.c @@ -0,0 +1,248 @@ +/** + ****************************************************************************** + * @addtogroup OpenPilotModules OpenPilot Modules + * @{ + * @addtogroup TxPIDModule TxPID Module + * @brief Optional module to tune PID settings using R/C transmitter. + * Updates PID settings in RAM in real-time using configured Accessory channels as controllers. + * @{ + * + * @file txpid.c + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2011. + * @brief Optional module to tune PID settings using R/C transmitter. + * + * @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 + */ + +/** + * Output object: StabilizationSettings + * + * This module will periodically update values of stabilization PID settings + * depending on configured input control channels. New values of stabilization + * settings are not saved to flash, but updated in RAM. It is expected that the + * module will be enabled only for tuning. When desired values are found, they + * can be read via GCS and saved permanently. Then this module should be + * disabled again. + * + * 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://wiki.openpilot.org/display/Doc/OpenPilot+Architecture + * + */ + +#include "openpilot.h" + +#include "txpidsettings.h" +#include "accessorydesired.h" +#include "stabilizationsettings.h" + +// +// Configuration +// +#define SAMPLE_PERIOD_MS 200 +#define TELEMETRY_UPDATE_PERIOD_MS 1000 // 0 = update on change + +// Sanity checks +#if (TXPIDSETTINGS_PIDS_NUMELEM != TXPIDSETTINGS_INPUTS_NUMELEM) || \ + (TXPIDSETTINGS_PIDS_NUMELEM != TXPIDSETTINGS_MIN_NUMELEM) || \ + (TXPIDSETTINGS_PIDS_NUMELEM != TXPIDSETTINGS_MAX_NUMELEM) +#error Invalid TxPID UAVObject definition (inconsistent number of field elements) +#endif + +// Private types + +// Private variables + +// Private functions +static void updatePIDs(UAVObjEvent* ev); +static uint8_t update(float *var, float val); +static float scale(float val, float min, float max); + +/** + * Initialise the module, called on startup + * \returns 0 on success or -1 if initialisation failed + */ +int32_t TxPIDInitialize(void) +{ + static UAVObjEvent ev; + ev.obj = AccessoryDesiredHandle(); + ev.instId = 0; + ev.event = 0; + + // Initialize settings object + TxPIDSettingsInitialize(); + +#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. + UAVObjMetadata metadata; + StabilizationSettingsGetMetadata(&metadata); + metadata.telemetryAcked = 0; + metadata.telemetryUpdateMode = UPDATEMODE_PERIODIC; + metadata.telemetryUpdatePeriod = TELEMETRY_UPDATE_PERIOD_MS; + StabilizationSettingsSetMetadata(&metadata); +#endif + + // Setup the callback function + EventPeriodicCallbackCreate(&ev, updatePIDs, SAMPLE_PERIOD_MS / portTICK_RATE_MS); + + return 0; +} + +/** + * Update PIDs callback function + */ +static void updatePIDs(UAVObjEvent* ev) +{ + if (ev->obj != AccessoryDesiredHandle()) + return; + + TxPIDSettingsData inst; + TxPIDSettingsGet(&inst); + StabilizationSettingsData stab; + StabilizationSettingsGet(&stab); + AccessoryDesiredData accessory; + + uint8_t needsUpdate = 0; + + // Loop through every enabled instance + for (uint8_t i = 0; i < TXPIDSETTINGS_PIDS_NUMELEM; i++) { + if (inst.PIDs[i] != TXPIDSETTINGS_PIDS_DISABLED) { + if (AccessoryDesiredInstGet(inst.Inputs[i] - TXPIDSETTINGS_INPUTS_ACCESSORY0, &accessory) == 0) { + float value = scale(accessory.AccessoryVal, inst.Min[i], inst.Max[i]); + + switch (inst.PIDs[i]) { + case TXPIDSETTINGS_PIDS_ROLLRATEKP: + needsUpdate |= update(&stab.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KP], value); + break; + case TXPIDSETTINGS_PIDS_ROLLRATEKI: + needsUpdate |= update(&stab.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KI], value); + break; + case TXPIDSETTINGS_PIDS_ROLLRATEKD: + needsUpdate |= update(&stab.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KD], value); + break; + case TXPIDSETTINGS_PIDS_ROLLATTITUDEKP: + needsUpdate |= update(&stab.RollPI[STABILIZATIONSETTINGS_ROLLPI_KP], value); + break; + case TXPIDSETTINGS_PIDS_ROLLATTITUDEKI: + needsUpdate |= update(&stab.RollPI[STABILIZATIONSETTINGS_ROLLPI_KI], value); + break; + case TXPIDSETTINGS_PIDS_PITCHRATEKP: + needsUpdate |= update(&stab.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KP], value); + break; + case TXPIDSETTINGS_PIDS_PITCHRATEKI: + needsUpdate |= update(&stab.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KI], value); + break; + case TXPIDSETTINGS_PIDS_PITCHRATEKD: + needsUpdate |= update(&stab.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KD], value); + break; + case TXPIDSETTINGS_PIDS_PITCHATTITUDEKP: + needsUpdate |= update(&stab.PitchPI[STABILIZATIONSETTINGS_PITCHPI_KP], value); + break; + case TXPIDSETTINGS_PIDS_PITCHATTITUDEKI: + needsUpdate |= update(&stab.PitchPI[STABILIZATIONSETTINGS_PITCHPI_KI], value); + break; + case TXPIDSETTINGS_PIDS_ROLLPITCHRATEKP: + needsUpdate |= update(&stab.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KP], value); + needsUpdate |= update(&stab.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KP], value); + break; + case TXPIDSETTINGS_PIDS_ROLLPITCHRATEKI: + needsUpdate |= update(&stab.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KI], value); + needsUpdate |= update(&stab.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KI], value); + break; + case TXPIDSETTINGS_PIDS_ROLLPITCHRATEKD: + needsUpdate |= update(&stab.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KD], value); + needsUpdate |= update(&stab.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KD], value); + break; + case TXPIDSETTINGS_PIDS_ROLLPITCHATTITUDEKP: + needsUpdate |= update(&stab.RollPI[STABILIZATIONSETTINGS_ROLLPI_KP], value); + needsUpdate |= update(&stab.PitchPI[STABILIZATIONSETTINGS_PITCHPI_KP], value); + break; + case TXPIDSETTINGS_PIDS_ROLLPITCHATTITUDEKI: + needsUpdate |= update(&stab.RollPI[STABILIZATIONSETTINGS_ROLLPI_KI], value); + needsUpdate |= update(&stab.PitchPI[STABILIZATIONSETTINGS_PITCHPI_KI], value); + break; + case TXPIDSETTINGS_PIDS_YAWRATEKP: + needsUpdate |= update(&stab.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_KP], value); + break; + case TXPIDSETTINGS_PIDS_YAWRATEKI: + needsUpdate |= update(&stab.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_KI], value); + break; + case TXPIDSETTINGS_PIDS_YAWRATEKD: + needsUpdate |= update(&stab.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_KD], value); + break; + case TXPIDSETTINGS_PIDS_YAWATTITUDEKP: + needsUpdate |= update(&stab.YawPI[STABILIZATIONSETTINGS_YAWPI_KP], value); + break; + case TXPIDSETTINGS_PIDS_YAWATTITUDEKI: + needsUpdate |= update(&stab.YawPI[STABILIZATIONSETTINGS_YAWPI_KI], value); + break; + } + } + } + } + if (needsUpdate) + StabilizationSettingsSet(&stab); +} + +/** + * Scale input value to [min..max] (min can be greater than max) + * \returns scaled value + */ +static float scale(float val, float min, float max) { + float v; + + if (min > max) { + val = -val; + v = min; + min = max; + max = v; + } + + v = min + (max - min) * (val + 1.0) * 0.5; + if (v > max) v = max; + if (v < min) v = min; + + return v; +} + +/** + * Updates value + * \returns 1 if update needed or 0 if not + */ +static uint8_t update(float *var, float val) { + if (*var != val) { + *var = val; + return 1; + } + return 0; +} + +/** + * @} + */ + +/** + * @} + */ diff --git a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro index 63eefa5bf..a4663e399 100644 --- a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro +++ b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro @@ -73,6 +73,7 @@ HEADERS += $$UAVOBJECT_SYNTHETICS/accessorydesired.h \ $$UAVOBJECT_SYNTHETICS/gcsreceiver.h \ $$UAVOBJECT_SYNTHETICS/receiveractivity.h \ $$UAVOBJECT_SYNTHETICS/attitudesettings.h \ + $$UAVOBJECT_SYNTHETICS/txpidsettings.h \ $$UAVOBJECT_SYNTHETICS/cameradesired.h SOURCES += $$UAVOBJECT_SYNTHETICS/accessorydesired.cpp \ @@ -126,4 +127,5 @@ SOURCES += $$UAVOBJECT_SYNTHETICS/accessorydesired.cpp \ $$UAVOBJECT_SYNTHETICS/gcsreceiver.cpp \ $$UAVOBJECT_SYNTHETICS/receiveractivity.cpp \ $$UAVOBJECT_SYNTHETICS/attitudesettings.cpp \ + $$UAVOBJECT_SYNTHETICS/txpidsettings.cpp \ $$UAVOBJECT_SYNTHETICS/cameradesired.cpp diff --git a/shared/uavobjectdefinition/hwsettings.xml b/shared/uavobjectdefinition/hwsettings.xml index 228d50b5e..6fdd75a21 100644 --- a/shared/uavobjectdefinition/hwsettings.xml +++ b/shared/uavobjectdefinition/hwsettings.xml @@ -9,7 +9,7 @@ - + diff --git a/shared/uavobjectdefinition/txpidsettings.xml b/shared/uavobjectdefinition/txpidsettings.xml new file mode 100644 index 000000000..5fd01aaad --- /dev/null +++ b/shared/uavobjectdefinition/txpidsettings.xml @@ -0,0 +1,25 @@ + + + Settings used by @ref TxPID optional module to tune PID settings using R/C transmitter + + + + + + + + + + +