Create new autotuning module which rotates through the axes for autotuning and

then computes new stabilization settings.

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 <pios_board_info.h>
+ 
+// 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);
+}
+
+/**
+ * @}
+ * @}
+ */

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

shared/uavobjectdefinition/flightstatus.xml

@@ -4,7 +4,7 @@
         <field name="Armed" units="" type="enum" elements="1" options="Disarmed,Arming,Armed" defaultvalue="Disarmed"/>
 
 	<!-- Note these enumerated values should be the same as ManualControlSettings -->
-        <field name="FlightMode" units="" type="enum" elements="1" options="Manual,Stabilized1,Stabilized2,Stabilized3,AltitudeHold,VelocityControl,PositionHold"/>
+        <field name="FlightMode" units="" type="enum" elements="1" options="Manual,Stabilized1,Stabilized2,Stabilized3,AltitudeHold,VelocityControl,PositionHold,Autotune"/>
 
         <access gcs="readwrite" flight="readwrite"/>
         <telemetrygcs acked="false" updatemode="manual" period="0"/>

shared/uavobjectdefinition/manualcontrolsettings.xml

@@ -24,7 +24,7 @@
 
 	<!-- Note these options values should be identical to those defined in FlightMode -->
         <field name="FlightModeNumber" units="" type="uint8" elements="1" defaultvalue="3"/>
-        <field name="FlightModePosition" units="" type="enum" elements="6" options="Manual,Stabilized1,Stabilized2,Stabilized3,AltitudeHold,VelocityControl,PositionHold" defaultvalue="Manual,Stabilized1,Stabilized2,Stabilized3,AltitudeHold,PositionHold"/>
+        <field name="FlightModePosition" units="" type="enum" elements="6" options="Manual,Stabilized1,Stabilized2,Stabilized3,AltitudeHold,VelocityControl,PositionHold,Autotune" defaultvalue="Manual,Stabilized1,Stabilized2,Stabilized3,AltitudeHold,PositionHold"/>
 
         <field name="ArmedTimeout" units="ms" type="uint16" elements="1" defaultvalue="30000"/>
         <access gcs="readwrite" flight="readwrite"/>