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