mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2025-03-01 18:29:16 +01:00
aad263bdd8
Besides of knob PID tuning it is now possible to use throttle channel to ramp-shape PID coefficients. This can be used to lower some PIDs on VTOL while sinking to prevent wobble. To use the feature select throttle as control input, choose throttle range max and min values, assign the instance to particular PID coefficient and define a range for it. When throttle is lower than defined throttle range min value (or higher than max), then min and max PID values will be used accordingly. Changing throttle from throttle min to max will linearly scale PID value. Note that it is possible to set MinPID > MaxPID. In that case increasing control input value will decrease the PID coefficient. Up to 3 independent instances can be configured. The number can be increased changing the UAVO definition, but at the cost of extra RAM.
270 lines
9.0 KiB
C
270 lines
9.0 KiB
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 "manualcontrolcommand.h"
|
|
#include "stabilizationsettings.h"
|
|
|
|
//
|
|
// Configuration
|
|
//
|
|
#define SAMPLE_PERIOD_MS 100
|
|
#define TELEMETRY_UPDATE_PERIOD_MS 1000 // 0 = update on change (default)
|
|
|
|
// Sanity checks
|
|
#if (TXPIDSETTINGS_PIDS_NUMELEM != TXPIDSETTINGS_INPUTS_NUMELEM) || \
|
|
(TXPIDSETTINGS_PIDS_NUMELEM != TXPIDSETTINGS_MINPID_NUMELEM) || \
|
|
(TXPIDSETTINGS_PIDS_NUMELEM != TXPIDSETTINGS_MAXPID_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 inMin, float inMax, float outMin, float outMax);
|
|
|
|
/**
|
|
* 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) {
|
|
|
|
float value;
|
|
if (inst.Inputs[i] == TXPIDSETTINGS_INPUTS_THROTTLE) {
|
|
ManualControlCommandThrottleGet(&value);
|
|
value = scale(value,
|
|
inst.ThrottleRange[TXPIDSETTINGS_THROTTLERANGE_MIN],
|
|
inst.ThrottleRange[TXPIDSETTINGS_THROTTLERANGE_MAX],
|
|
inst.MinPID[i], inst.MaxPID[i]);
|
|
} else if (AccessoryDesiredInstGet(inst.Inputs[i] - TXPIDSETTINGS_INPUTS_ACCESSORY0, &accessory) == 0) {
|
|
value = scale(accessory.AccessoryVal, -1.0, 1.0, inst.MinPID[i], inst.MaxPID[i]);
|
|
} else {
|
|
continue;
|
|
}
|
|
|
|
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);
|
|
}
|
|
|
|
/**
|
|
* Scales input val from [inMin..inMax] range to [outMin..outMax].
|
|
* If val is out of input range (inMin <= inMax), it will be bound.
|
|
* (outMin > outMax) is ok, in that case output will be decreasing.
|
|
*
|
|
* \returns scaled value
|
|
*/
|
|
static float scale(float val, float inMin, float inMax, float outMin, float outMax)
|
|
{
|
|
// bound input value
|
|
if (val > inMax) val = inMax;
|
|
if (val < inMin) val = inMin;
|
|
|
|
// normalize input value to [0..1]
|
|
if (inMax <= inMin)
|
|
val = 0.0;
|
|
else
|
|
val = (val - inMin) / (inMax - inMin);
|
|
|
|
// update output bounds
|
|
if (outMin > outMax) {
|
|
float t = outMin;
|
|
outMin = outMax;
|
|
outMax = t;
|
|
val = 1.0 - val;
|
|
}
|
|
|
|
return (outMax - outMin) * val + outMin;
|
|
}
|
|
|
|
/**
|
|
* Updates var using val if needed.
|
|
* \returns 1 if updated, 0 otherwise
|
|
*/
|
|
static uint8_t update(float *var, float val)
|
|
{
|
|
if (*var != val) {
|
|
*var = val;
|
|
return 1;
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
/**
|
|
* @}
|
|
*/
|
|
|
|
/**
|
|
* @}
|
|
*/
|