mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2024-11-29 07:24:13 +01:00
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.
This commit is contained in:
Oleg Semyonov
parent
abe5136de0
commit
d923117c25
@ -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
|
||||
|
||||
|
||||
@ -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();
|
||||
|
||||
42
flight/Modules/TxPID/inc/txpid.h
Normal file
42
flight/Modules/TxPID/inc/txpid.h
Normal file
@ -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
|
||||
|
||||
/**
|
||||
* @}
|
||||
* @}
|
||||
*/
|
||||
248
flight/Modules/TxPID/txpid.c
Normal file
248
flight/Modules/TxPID/txpid.c
Normal file
@ -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;
|
||||
}
|
||||
|
||||
/**
|
||||
* @}
|
||||
*/
|
||||
|
||||
/**
|
||||
* @}
|
||||
*/
|
||||
@ -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
|
||||
|
||||
@ -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"/>
|
||||
|
||||
25
shared/uavobjectdefinition/txpidsettings.xml
Normal file
25
shared/uavobjectdefinition/txpidsettings.xml
Normal file
@ -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>
|
||||
Loading…
Reference in New Issue
Block a user