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

@@ -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
 

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();

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
+
+/**
+  * @}
+  * @}
+  */

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

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

shared/uavobjectdefinition/hwsettings.xml

@@ -9,7 +9,7 @@
 		<field name="OP_MainPort" units="function" type="enum" elements="1" options="Disabled,Telemetry" defaultvalue="Telemetry"/>
 		<field name="OP_RcvrPort" units="function" type="enum" elements="1" options="Disabled,Debug,Spektrum1,PWM,PPM" defaultvalue="PWM"/>
 
-		<field name="OptionalModules" units="" type="enum" elementnames="CameraStabilization,GPS" options="Disabled,Enabled" defaultvalue="Disabled"/>
+		<field name="OptionalModules" units="" type="enum" elementnames="CameraStabilization,GPS,TxPID" options="Disabled,Enabled" defaultvalue="Disabled"/>
 		<field name="DSMxBind" units=""  type="uint8"  elements="1" defaultvalue="0"/>
 
 		<access gcs="readwrite" flight="readwrite"/>

shared/uavobjectdefinition/txpidsettings.xml

@@ -0,0 +1,25 @@
+<xml>
+    <object name="TxPIDSettings" singleinstance="true" settings="true">
+        <description>Settings used by @ref TxPID optional module to tune PID settings using R/C transmitter</description>
+        <field name="PIDs" units="option" type="enum"
+		elementnames="Instance1,Instance2,Instance3"
+		options="Disabled,
+			Roll Rate.Kp, Pitch Rate.Kp, Roll+Pitch Rate.Kp, Yaw Rate.Kp, 
+			Roll Rate.Ki, Pitch Rate.Ki, Roll+Pitch Rate.Ki, Yaw Rate.Ki,
+			Roll Rate.Kd, Pitch Rate.Kd, Roll+Pitch Rate.Kd, Yaw Rate.Kd,
+			Roll Attitude.Kp, Pitch Attitude.Kp, Roll+Pitch Attitude.Kp, Yaw Attitude.Kp,
+			Roll Attitude.Ki, Pitch Attitude.Ki, Roll+Pitch Attitude.Ki, Yaw Attitude.Ki"
+		defaultvalue="Disabled"/>
+        <field name="Inputs" units="channel" type="enum"
+		elementnames="Instance1,Instance2,Instance3"
+		options="Accessory0,Accessory1,Accessory2,Accessory3,Accessory4,Accessory5"
+		defaultvalue="Accessory0"/>
+        <field name="Min" units="" type="float" elementnames="Instance1,Instance2,Instance3" defaultvalue="0"/>
+        <field name="Max" units="" type="float" elementnames="Instance1,Instance2,Instance3" defaultvalue="0"/>
+
+	<access gcs="readwrite" flight="readwrite"/>
+	<telemetrygcs acked="true" updatemode="onchange" period="0"/>
+	<telemetryflight acked="true" updatemode="onchange" period="0"/>
+	<logging updatemode="never" period="0"/>
+    </object>
+</xml>