LibrePilot/flight/Modules/TxPID/txpid.c

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

/**
  * @}
  */

/**
 * @}
 */