rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-11-29 07:24:13 +01:00
Code Issues Releases Activity

Merge branch 'pidt1' into test2

Browse Source 
Conflicts:
	flight/Modules/ManualControl/inc/manualcontrol.h
	ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro
	shared/uavobjectdefinition/flightstatus.xml
	shared/uavobjectdefinition/hwsettings.xml
	shared/uavobjectdefinition/manualcontrolsettings.xml
	shared/uavobjectdefinition/taskinfo.xml
This commit is contained in:
James Cotton 2012-09-10 14:32:54 -05:00
commit 38e9d105aa
33 changed files with 2622 additions and 257 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
flight/CopterControl/Makefile
View File

@ -68,6 +68,7 @@ USE_GPS ?= YES
USE_TXPID ?= YES
USE_I2C ?= NO
USE_ALTITUDE ?= NO
USE_AUTOTUNE ?= YES
TEST_FAULTS ?= NO
# List of optional modules to include
@ -94,6 +95,9 @@ endif
ifeq ($(TEST_FAULTS), YES)
OPTMODULES += Fault
endif
ifeq ($(USE_AUTOTUNE), YES)
OPTMODULES += Autotune
endif
# List of mandatory modules to include
MODULES = Attitude Stabilization Actuator ManualControl FirmwareIAP
@ -111,6 +115,8 @@ OPTESTS = ./Tests
OPMODULEDIR = ../Modules
FLIGHTLIB = ../Libraries
FLIGHTLIBINC = $(FLIGHTLIB)/inc
MATHLIB = ../Libraries/math
MATHLIBINC = ../Libraries/math
PIOS = ../PiOS
PIOSINC = $(PIOS)/inc
PIOSSTM32F10X = $(PIOS)/STM32F10x
@ -204,6 +210,8 @@ SRC += $(OPUAVSYNTHDIR)/gpssettings.c
SRC += $(OPUAVSYNTHDIR)/hwsettings.c
SRC += $(OPUAVSYNTHDIR)/gcsreceiver.c
SRC += $(OPUAVSYNTHDIR)/receiveractivity.c
SRC += $(OPUAVSYNTHDIR)/relaytuningsettings.c
SRC += $(OPUAVSYNTHDIR)/relaytuning.c
SRC += $(OPUAVSYNTHDIR)/taskinfo.c
SRC += $(OPUAVSYNTHDIR)/mixerstatus.c
SRC += $(OPUAVSYNTHDIR)/ratedesired.c
@ -264,6 +272,8 @@ SRC += $(PIOSCOMMON)/printf-stdarg.c
SRC += $(FLIGHTLIB)/fifo_buffer.c
SRC += $(FLIGHTLIB)/CoordinateConversions.c
SRC += $(FLIGHTLIB)/taskmonitor.c
SRC += $(MATHLIB)/sin_lookup.c
SRC += $(MATHLIB)/pid.c
## CMSIS for STM32
SRC += $(CMSISDIR)/core_cm3.c
@ -364,6 +374,7 @@ EXTRAINCDIRS += $(OPUAVSYNTHDIR)
EXTRAINCDIRS += $(PIOS)
EXTRAINCDIRS += $(PIOSINC)
EXTRAINCDIRS += $(FLIGHTLIBINC)
EXTRAINCDIRS += $(MATHLIBINC)
EXTRAINCDIRS += $(PIOSSTM32F10X)
EXTRAINCDIRS += $(PIOSCOMMON)
EXTRAINCDIRS += $(PIOSBOARDS)

101
flight/Libraries/math/pid.c Normal file
View File

@ -0,0 +1,101 @@
/**
******************************************************************************
* @addtogroup OpenPilot Math Utilities
* @{
* @addtogroup Sine and cosine methods that use a cached lookup table
* @{
*
* @file pid.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
* @brief Methods to work with PID structure
*
* @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
*/
#include "openpilot.h"
#include "pid.h"
//! Private method
static float bound(float val, float range);
float pid_apply(struct pid *pid, const float err, float dT)
{
float diff = (err - pid->lastErr);
float dterm = 0;
pid->lastErr = err;
// Scale up accumulator by 1000 while computing to avoid losing precision
pid->iAccumulator += err * (pid->i * dT * 1000.0f);
pid->iAccumulator = bound(pid->iAccumulator, pid->iLim * 1000.0f);
// Calculate DT1 term, fixed T1 timeconstant
if(pid->d && dT)
{
dterm = pid->lastDer + dT / ( dT + 7.9577e-3f) * ((diff * pid->d / dT) - pid->lastDer);
pid->lastDer = dterm; // ^ set constant to 1/(2*pi*f_cutoff)
} // 7.9577e-3 means 20 Hz f_cutoff
return ((err * pid->p) + pid->iAccumulator / 1000.0f + dterm);
}
/**
* Reset a bit
* @param[in] pid The pid to reset
*/
void pid_zero(struct pid *pid)
{
if (!pid)
return;
pid->iAccumulator = 0;
pid->lastErr = 0;
pid->lastDer = 0;
}
/**
* Configure the settings for a pid structure
* @param[out] pid The PID structure to configure
* @param[in] p The proportional term
* @param[in] i The integral term
* @param[in] d The derivative term
*/
void pid_configure(struct pid *pid, float p, float i, float d, float iLim)
{
if (!pid)
return;
pid->p = p;
pid->i = i;
pid->d = d;
pid->iLim = iLim;
}
/**
* Bound input value between limits
*/
static float bound(float val, float range)
{
if(val < -range) {
val = -range;
} else if(val > range) {
val = range;
}
return val;
}

50
flight/Libraries/math/pid.h Normal file
View File

@ -0,0 +1,50 @@
/**
******************************************************************************
* @addtogroup OpenPilot Math Utilities
* @{
* @addtogroup Sine and cosine methods that use a cached lookup table
* @{
*
* @file pid.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
* @brief Methods to work with PID structure
*
* @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 PID_H
#define PID_H
//!
struct pid {
float p;
float i;
float d;
float iLim;
float iAccumulator;
float lastErr;
float lastDer;
};
//! Methods to use the pid structures
float pid_apply(struct pid *pid, const float err, float dT);
void pid_zero(struct pid *pid);
void pid_configure(struct pid *pid, float p, float i, float d, float iLim);
#endif /* PID_H */

142
flight/Libraries/math/sin_lookup.c Normal file
View File

@ -0,0 +1,142 @@
/**
******************************************************************************
* @addtogroup OpenPilot Math Utilities
* @{
* @addtogroup Sine and cosine methods that use a cached lookup table
* @{
*
* @file sin_lookup.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
* @brief Sine lookup table from flash with 1 degree resolution
*
* @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
*/
#include "openpilot.h"
#include "math.h"
#include "stdbool.h"
#include "stdint.h"
#define FLASH_TABLE
#ifdef FLASH_TABLE
/** Version of the code which precomputes the lookup table in flash **/
// This is a precomputed sin lookup table over 90 degrees that
const float sin_table[] = {
0.000000f,0.017452f,0.034899f,0.052336f,0.069756f,0.087156f,0.104528f,0.121869f,0.139173f,0.156434f,
0.173648f,0.190809f,0.207912f,0.224951f,0.241922f,0.258819f,0.275637f,0.292372f,0.309017f,0.325568f,
0.342020f,0.358368f,0.374607f,0.390731f,0.406737f,0.422618f,0.438371f,0.453990f,0.469472f,0.484810f,
0.500000f,0.515038f,0.529919f,0.544639f,0.559193f,0.573576f,0.587785f,0.601815f,0.615661f,0.629320f,
0.642788f,0.656059f,0.669131f,0.681998f,0.694658f,0.707107f,0.719340f,0.731354f,0.743145f,0.754710f,
0.766044f,0.777146f,0.788011f,0.798636f,0.809017f,0.819152f,0.829038f,0.838671f,0.848048f,0.857167f,
0.866025f,0.874620f,0.882948f,0.891007f,0.898794f,0.906308f,0.913545f,0.920505f,0.927184f,0.933580f,
0.939693f,0.945519f,0.951057f,0.956305f,0.961262f,0.965926f,0.970296f,0.974370f,0.978148f,0.981627f,
0.984808f,0.987688f,0.990268f,0.992546f,0.994522f,0.996195f,0.997564f,0.998630f,0.999391f,0.999848f,
1.000000f,0.999848f,0.999391f,0.998630f,0.997564f,0.996195f,0.994522f,0.992546f,0.990268f,0.987688f,
0.984808f,0.981627f,0.978148f,0.974370f,0.970296f,0.965926f,0.961262f,0.956305f,0.951057f,0.945519f,
0.939693f,0.933580f,0.927184f,0.920505f,0.913545f,0.906308f,0.898794f,0.891007f,0.882948f,0.874620f,
0.866025f,0.857167f,0.848048f,0.838671f,0.829038f,0.819152f,0.809017f,0.798636f,0.788011f,0.777146f,
0.766044f,0.754710f,0.743145f,0.731354f,0.719340f,0.707107f,0.694658f,0.681998f,0.669131f,0.656059f,
0.642788f,0.629320f,0.615661f,0.601815f,0.587785f,0.573576f,0.559193f,0.544639f,0.529919f,0.515038f,
0.500000f,0.484810f,0.469472f,0.453990f,0.438371f,0.422618f,0.406737f,0.390731f,0.374607f,0.358368f,
0.342020f,0.325568f,0.309017f,0.292372f,0.275637f,0.258819f,0.241922f,0.224951f,0.207912f,0.190809f,
0.173648f,0.156434f,0.139173f,0.121869f,0.104528f,0.087156f,0.069756f,0.052336f,0.034899f,0.017452f
};
int sin_lookup_initalize()
{
return 0;
}
#else
/** Version of the code which allocates the lookup table in heap **/
const int SIN_RESOLUTION = 180;
static float *sin_table;
int sin_lookup_initalize()
{
// Previously initialized
if (sin_table)
return 0;
sin_table = (float *) pvPortMalloc(sizeof(float) * SIN_RESOLUTION);
if (sin_table == NULL)
return -1;
for(uint32_t i = 0; i < 180; i++)
sin_table[i] = sinf((float)i * 2 * M_PI / 360.0f);
return 0;
}
#endif
/**
* Use the lookup table to return sine(angle) where angle is in radians
* to save flash this cheats and uses trig functions to only save
* 90 values
* @param[in] angle Angle in degrees
* @returns sin(angle)
*/
float sin_lookup_deg(float angle)
{
if (sin_table == NULL)
return 0;
int i_ang = ((int32_t) angle) % 360;
if (i_ang >= 180) // for 180 to 360 deg
return -sin_table[i_ang - 180];
else // for 0 to 179 deg
return sin_table[i_ang];
return 0;
}
/**
* Get cos(angle) using the sine lookup table
* @param[in] angle Angle in degrees
* @returns cos(angle)
*/
float cos_lookup_deg(float angle)
{
return sin_lookup_deg(angle + 90);
}
/**
* Use the lookup table to return sine(angle) where angle is in radians
* @param[in] angle Angle in radians
* @returns sin(angle)
*/
float sin_lookup_rad(float angle)
{
int degrees = angle * 180.0f / M_PI;
return sin_lookup_deg(degrees);
}
/**
* Use the lookup table to return sine(angle) where angle is in radians
* @param[in] angle Angle in radians
* @returns cos(angle)
*/
float cos_lookup_rad(float angle)
{
int degrees = angle * 180.0f / M_PI;
return cos_lookup_deg(degrees);
}

40
flight/Libraries/math/sin_lookup.h Normal file
View File

@ -0,0 +1,40 @@
/**
******************************************************************************
* @addtogroup OpenPilot Math Utilities
* @{
* @addtogroup Sine and cosine methods that use a cached lookup table
* @{
*
* @file sin_lookup.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
* @brief Sine lookup table from flash with 1 degree resolution
*
* @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 SIN_LOOKUP_H
#define SIN_LOOKUP_H
int sin_lookup_initalize();
float sin_lookup_deg(float angle);
float cos_lookup_deg(float angle);
float sin_lookup_rad(float angle);
float cos_lookup_rad(float angle);
#endif

333
flight/Modules/Autotune/autotune.c Normal file
View File

@ -0,0 +1,333 @@
/**
******************************************************************************
* @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 "hwsettings.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;
static bool autotuneEnabled;
// 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
#ifdef MODULE_AUTOTUNE_BUILTIN
autotuneEnabled = true;
#else
HwSettingsInitialize();
uint8_t optionalModules[HWSETTINGS_OPTIONALMODULES_NUMELEM];
HwSettingsOptionalModulesGet(optionalModules);
if (optionalModules[HWSETTINGS_OPTIONALMODULES_AUTOTUNE] == HWSETTINGS_OPTIONALMODULES_ENABLED)
autotuneEnabled = true;
else
autotuneEnabled = false;
#endif
return 0;
}
/**
* Initialise the module, called on startup
* \returns 0 on success or -1 if initialisation failed
*/
int32_t AutotuneStart(void)
{
// Start main task if it is enabled
if(autotuneEnabled) {
xTaskCreate(AutotuneTask, (signed char *)"Autotune", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &taskHandle);
TaskMonitorAdd(TASKINFO_RUNNING_AUTOTUNE, taskHandle);
PIOS_WDG_RegisterFlag(PIOS_WDG_AUTOTUNE);
}
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) {
PIOS_WDG_UpdateFlag(PIOS_WDG_AUTOTUNE);
// 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 = 30000;
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);
RelayTuningSettingsData relaySettings;
RelayTuningSettingsGet(&relaySettings);
bool rate = relaySettings.Mode == RELAYTUNINGSETTINGS_MODE_RATE;
if (rate) { // rate mode
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE;
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE;
stabDesired.Roll = manualControl.Roll * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_ROLL];
stabDesired.Pitch = manualControl.Pitch * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_PITCH];
} else {
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.Roll = manualControl.Roll * stabSettings.RollMax;
stabDesired.Pitch = manualControl.Pitch * stabSettings.PitchMax;
}
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE;
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] = rate ? STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE :
STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE;
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] = rate ? STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE :
STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE;
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;
// For now just run over roll and pitch
for (uint i = 0; i < 2; i++) {
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 = 4.0f / M_PI / relayTuning.Gain[i]; // 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;
}
}
switch(relaySettings.Behavior) {
case RELAYTUNINGSETTINGS_BEHAVIOR_MEASURE:
// Just measure, don't update the stab settings
break;
case RELAYTUNINGSETTINGS_BEHAVIOR_COMPUTE:
StabilizationSettingsSet(&stabSettings);
break;
case RELAYTUNINGSETTINGS_BEHAVIOR_SAVE:
StabilizationSettingsSet(&stabSettings);
UAVObjSave(StabilizationSettingsHandle(), 0);
break;
}
}
/**
* @}
* @}
*/

37
flight/Modules/Autotune/inc/autotune.h Normal file
View File

@ -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

5
flight/Modules/ManualControl/inc/manualcontrol.h
View File

@ -32,7 +32,7 @@
#include "manualcontrolcommand.h"
typedef enum {FLIGHTMODE_UNDEFINED = 0, FLIGHTMODE_MANUAL = 1, FLIGHTMODE_STABILIZED = 2, FLIGHTMODE_GUIDANCE = 3} flightmode_path;
typedef enum {FLIGHTMODE_UNDEFINED = 0, FLIGHTMODE_MANUAL = 1, FLIGHTMODE_STABILIZED = 2, FLIGHTMODE_GUIDANCE = 3, FLIGHTMODE_TUNING = 4} flightmode_path;
#define PARSE_FLIGHT_MODE(x) ( \
(x == FLIGHTSTATUS_FLIGHTMODE_MANUAL) ? FLIGHTMODE_MANUAL : \
@ -43,7 +43,8 @@ typedef enum {FLIGHTMODE_UNDEFINED = 0, FLIGHTMODE_MANUAL = 1, FLIGHTMODE_STABIL
(x == FLIGHTSTATUS_FLIGHTMODE_VELOCITYCONTROL) ? FLIGHTMODE_GUIDANCE : \
(x == FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD) ? FLIGHTMODE_GUIDANCE : \
(x == FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER) ? FLIGHTMODE_GUIDANCE : \
(x == FLIGHTSTATUS_FLIGHTMODE_RTH) ? FLIGHTMODE_GUIDANCE : FLIGHTMODE_UNDEFINED \
(x == FLIGHTSTATUS_FLIGHTMODE_RTH) ? FLIGHTMODE_GUIDANCE : \
(x == FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE) ? FLIGHTMODE_TUNING : FLIGHTMODE_UNDEFINED \
)
int32_t ManualControlInitialize();

10
flight/Modules/ManualControl/manualcontrol.c
View File

@ -419,6 +419,10 @@ static void manualControlTask(void *parameters)
case FLIGHTMODE_STABILIZED:
updateStabilizationDesired(&cmd, &settings);
break;
case FLIGHTMODE_TUNING:
// Tuning takes settings directly from manualcontrolcommand. No need to
// call anything else. This just avoids errors.
break;
case FLIGHTMODE_GUIDANCE:
switch(flightStatus.FlightMode) {
case FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD:
@ -627,6 +631,8 @@ static void updateStabilizationDesired(ManualControlCommandData * cmd, ManualCon
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Roll * stabSettings.RollMax :
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd->Roll * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_ROLL] :
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd->Roll :
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? cmd->Roll * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_ROLL] :
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd->Roll * stabSettings.RollMax :
0; // this is an invalid mode
;
stabilization.Pitch = (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Pitch :
@ -635,6 +641,8 @@ static void updateStabilizationDesired(ManualControlCommandData * cmd, ManualCon
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Pitch * stabSettings.PitchMax :
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd->Pitch * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_PITCH] :
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd->Pitch :
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? cmd->Pitch * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_PITCH] :
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd->Pitch * stabSettings.PitchMax :
0; // this is an invalid mode
stabilization.Yaw = (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Yaw :
@ -643,6 +651,8 @@ static void updateStabilizationDesired(ManualControlCommandData * cmd, ManualCon
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Yaw * stabSettings.YawMax :
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd->Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW] :
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd->Yaw :
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? cmd->Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW] :
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd->Yaw * stabSettings.YawMax :
0; // this is an invalid mode
stabilization.Throttle = (cmd->Throttle < 0) ? -1 : cmd->Throttle;

39
flight/Modules/Stabilization/inc/relay_tuning.h Normal file
View File

@ -0,0 +1,39 @@
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup StabilizationModule Stabilization Module
* @brief Relay tuning controller
* @note This object updates the @ref ActuatorDesired "Actuator Desired" based on the
* PID loops on the @ref AttitudeDesired "Attitude Desired" and @ref AttitudeActual "Attitude Actual"
* @{
*
* @file relay_tuning.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
* @brief Attitude stabilization module.
*
* @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 RELAY_TUNING_H
#define RELAY_TUNING_H
int stabilization_relay_rate(float err, float *output, int axis, bool reinit);
#endif

2
flight/Modules/Stabilization/inc/stabilization.h
View File

@ -33,6 +33,8 @@
#ifndef STABILIZATION_H
#define STABILIZATION_H
enum {ROLL,PITCH,YAW,MAX_AXES};
int32_t StabilizationInitialize();
#endif // STABILIZATION_H

42
flight/Modules/Stabilization/inc/virtualflybar.h Normal file
View File

@ -0,0 +1,42 @@
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup StabilizationModule Stabilization Module
* @brief Virtual flybar mode
* @note This file implements the logic for a virtual flybar
* @{
*
* @file virtualflybar.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
* @brief Attitude stabilization module.
*
* @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 VIRTUALFLYBAR_H
#define VIRTUALFLYBAR_H
#include "openpilot.h"
#include "stabilizationsettings.h"
int stabilization_virtual_flybar(float gyro, float command, float *output, float dT, bool reinit, uint32_t axis, StabilizationSettingsData *settings);
int stabilization_virtual_flybar_pirocomp(float z_gyro, float dT);
#endif /* VIRTUALFLYBAR_H */

151
flight/Modules/Stabilization/relay_tuning.c Normal file
View File

@ -0,0 +1,151 @@
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup StabilizationModule Stabilization Module
* @brief Relay tuning controller
* @note This object updates the @ref ActuatorDesired "Actuator Desired" based on the
* PID loops on the @ref AttitudeDesired "Attitude Desired" and @ref AttitudeActual "Attitude Actual"
* @{
*
* @file stabilization.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief Attitude stabilization module.
*
* @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
*/
#include "openpilot.h"
#include "relaytuning.h"
#include "relaytuningsettings.h"
#include "sin_lookup.h"
//! Private variables
static const int SIN_RESOLUTION = 180;
#define MAX_AXES 3
/**
* Apply a step function for the stabilization controller and monitor the
* result
*
* Used to Replace the rate PID with a relay to measure the critical properties of this axis
* i.e. period and gain
*/
int stabilization_relay_rate(float error, float *output, int axis, bool reinit)
{
RelayTuningData relay;
RelayTuningGet(&relay);
static portTickType lastHighTime;
static portTickType lastLowTime;
static float accum_sin, accum_cos;
static uint32_t accumulated = 0;
const uint16_t DEGLITCH_TIME = 20; // ms
const float AMPLITUDE_ALPHA = 0.95;
const float PERIOD_ALPHA = 0.95;
portTickType thisTime = xTaskGetTickCount();
static bool rateRelayRunning[MAX_AXES];
// This indicates the current estimate of the smoothed error. So when it is high
// we are waiting for it to go low.
static bool high = false;
// On first run initialize estimates to something reasonable
if(reinit) {
rateRelayRunning[axis] = false;
relay.Period[axis] = 200;
relay.Gain[axis] = 0;
accum_sin = 0;
accum_cos = 0;
accumulated = 0;
// These should get reinitialized anyway
high = true;
lastHighTime = thisTime;
lastLowTime = thisTime;
RelayTuningSet(&relay);
}
RelayTuningSettingsData relaySettings;
RelayTuningSettingsGet(&relaySettings);
// Compute output, simple threshold on error
*output = high ? relaySettings.Amplitude : -relaySettings.Amplitude;
/**** The code below here is to estimate the properties of the oscillation ****/
// Make sure the period can't go below limit
if (relay.Period[axis] < DEGLITCH_TIME)
relay.Period[axis] = DEGLITCH_TIME;
// Project the error onto a sine and cosine of the same frequency
// to accumulate the average amplitude
int32_t dT = thisTime - lastHighTime;
float phase = ((float)360 * (float)dT) / relay.Period[axis];
if(phase >= 360)
phase = 0;
accum_sin += sin_lookup_deg(phase) * error;
accum_cos += sin_lookup_deg(phase + 90) * error;
accumulated ++;
// Make sure we've had enough time since last transition then check for a change in the output
bool time_hysteresis = (high ? (thisTime - lastHighTime) : (thisTime - lastLowTime)) > DEGLITCH_TIME;
if ( !high && time_hysteresis && error > relaySettings.HysteresisThresh ){
/* POSITIVE CROSSING DETECTED */
float this_amplitude = 2 * sqrtf(accum_sin*accum_sin + accum_cos*accum_cos) / accumulated;
float this_gain = this_amplitude / relaySettings.Amplitude;
accumulated = 0;
accum_sin = 0;
accum_cos = 0;
if(rateRelayRunning[axis] == false) {
rateRelayRunning[axis] = true;
relay.Period[axis] = 200;
relay.Gain[axis] = 0;
} else {
// Low pass filter each amplitude and period
relay.Gain[axis] = relay.Gain[axis] * AMPLITUDE_ALPHA + this_gain * (1 - AMPLITUDE_ALPHA);
relay.Period[axis] = relay.Period[axis] * PERIOD_ALPHA + dT * (1 - PERIOD_ALPHA);
}
lastHighTime = thisTime;
high = true;
RelayTuningSet(&relay);
} else if ( high && time_hysteresis && error < -relaySettings.HysteresisThresh ) {
/* FALLING CROSSING DETECTED */
lastLowTime = thisTime;
high = false;
}
return 0;
}

429
flight/Modules/Stabilization/stabilization.c
View File

@ -36,12 +36,22 @@
#include "stabilizationsettings.h"
#include "actuatordesired.h"
#include "ratedesired.h"
#include "relaytuning.h"
#include "relaytuningsettings.h"
#include "stabilizationdesired.h"
#include "attitudeactual.h"
#include "gyros.h"
#include "flightstatus.h"
#include "manualcontrol.h" // Just to get a macro
// Math libraries
#include "CoordinateConversions.h"
#include "pid.h"
#include "sin_lookup.h"
// Includes for various stabilization algorithms
#include "relay_tuning.h"
#include "virtualflybar.h"
// Private constants
#define MAX_QUEUE_SIZE 1
@ -55,44 +65,26 @@
#define TASK_PRIORITY (tskIDLE_PRIORITY+4)
#define FAILSAFE_TIMEOUT_MS 30
enum {PID_RATE_ROLL, PID_RATE_PITCH, PID_RATE_YAW, PID_ROLL, PID_PITCH, PID_YAW, PID_VBAR_ROLL, PID_VBAR_PITCH, PID_VBAR_YAW, PID_MAX};
enum {PID_RATE_ROLL, PID_RATE_PITCH, PID_RATE_YAW, PID_ROLL, PID_PITCH, PID_YAW, PID_MAX};
enum {ROLL,PITCH,YAW,MAX_AXES};
// Private types
typedef struct {
float p;
float i;
float d;
float iLim;
float iAccumulator;
float lastErr;
} pid_type;
// Private variables
static xTaskHandle taskHandle;
static StabilizationSettingsData settings;
static xQueueHandle queue;
float gyro_alpha = 0;
float gyro_filtered[3] = {0,0,0};
float axis_lock_accum[3] = {0,0,0};
float vbar_sensitivity[3] = {1, 1, 1};
uint8_t max_axis_lock = 0;
uint8_t max_axislock_rate = 0;
float weak_leveling_kp = 0;
uint8_t weak_leveling_max = 0;
bool lowThrottleZeroIntegral;
float vbar_integral[3] = {0, 0, 0};
float vbar_decay = 0.991f;
pid_type pids[PID_MAX];
int8_t vbar_gyros_suppress;
bool vbar_piro_comp = false;
struct pid pids[PID_MAX];
// Private functions
static void stabilizationTask(void* parameters);
static float ApplyPid(pid_type * pid, const float err, float dT);
static float bound(float val);
static float bound(float val, float range);
static void ZeroPids(void);
static void SettingsUpdatedCb(UAVObjEvent * ev);
@ -108,10 +100,10 @@ int32_t StabilizationStart()
// Listen for updates.
// AttitudeActualConnectQueue(queue);
GyrosConnectQueue(queue);
StabilizationSettingsConnectCallback(SettingsUpdatedCb);
SettingsUpdatedCb(StabilizationSettingsHandle());
// Start main task
xTaskCreate(stabilizationTask, (signed char*)"Stabilization", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &taskHandle);
TaskMonitorAdd(TASKINFO_RUNNING_STABILIZATION, taskHandle);
@ -131,6 +123,11 @@ int32_t StabilizationInitialize()
RateDesiredInitialize();
#endif
// Code required for relay tuning
sin_lookup_initalize();
RelayTuningSettingsInitialize();
RelayTuningInitialize();
return 0;
}
@ -142,73 +139,73 @@ MODULE_INITCALL(StabilizationInitialize, StabilizationStart)
static void stabilizationTask(void* parameters)
{
UAVObjEvent ev;
uint32_t timeval = PIOS_DELAY_GetRaw();
ActuatorDesiredData actuatorDesired;
StabilizationDesiredData stabDesired;
RateDesiredData rateDesired;
AttitudeActualData attitudeActual;
GyrosData gyrosData;
FlightStatusData flightStatus;
SettingsUpdatedCb((UAVObjEvent *) NULL);
// Main task loop
ZeroPids();
while(1) {
float dT;
PIOS_WDG_UpdateFlag(PIOS_WDG_STABILIZATION);
// Wait until the AttitudeRaw object is updated, if a timeout then go to failsafe
if ( xQueueReceive(queue, &ev, FAILSAFE_TIMEOUT_MS / portTICK_RATE_MS) != pdTRUE )
{
AlarmsSet(SYSTEMALARMS_ALARM_STABILIZATION,SYSTEMALARMS_ALARM_WARNING);
continue;
}
dT = PIOS_DELAY_DiffuS(timeval) * 1.0e-6f;
timeval = PIOS_DELAY_GetRaw();
FlightStatusGet(&flightStatus);
StabilizationDesiredGet(&stabDesired);
AttitudeActualGet(&attitudeActual);
GyrosGet(&gyrosData);
#if defined(DIAGNOSTICS)
RateDesiredGet(&rateDesired);
#endif
#if defined(PIOS_QUATERNION_STABILIZATION)
// Quaternion calculation of error in each axis. Uses more memory.
float rpy_desired[3];
float q_desired[4];
float q_error[4];
float local_error[3];
// Essentially zero errors for anything in rate or none
if(stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE)
rpy_desired[0] = stabDesired.Roll;
else
rpy_desired[0] = attitudeActual.Roll;
if(stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE)
rpy_desired[1] = stabDesired.Pitch;
else
rpy_desired[1] = attitudeActual.Pitch;
if(stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE)
rpy_desired[2] = stabDesired.Yaw;
else
rpy_desired[2] = attitudeActual.Yaw;
RPY2Quaternion(rpy_desired, q_desired);
quat_inverse(q_desired);
quat_mult(q_desired, &attitudeActual.q1, q_error);
quat_inverse(q_error);
Quaternion2RPY(q_error, local_error);
#else
// Simpler algorithm for CC, less memory
float local_error[3] = {stabDesired.Roll - attitudeActual.Roll,
@ -217,7 +214,7 @@ static void stabilizationTask(void* parameters)
local_error[2] = fmodf(local_error[2] + 180, 360) - 180;
#endif
float gyro_filtered[3];
gyro_filtered[0] = gyro_filtered[0] * gyro_alpha + gyrosData.x * (1 - gyro_alpha);
gyro_filtered[1] = gyro_filtered[1] * gyro_alpha + gyrosData.y * (1 - gyro_alpha);
gyro_filtered[2] = gyro_filtered[2] * gyro_alpha + gyrosData.z * (1 - gyro_alpha);
@ -226,49 +223,79 @@ static void stabilizationTask(void* parameters)
float *actuatorDesiredAxis = &actuatorDesired.Roll;
float *rateDesiredAxis = &rateDesired.Roll;
//Calculate desired rate
ActuatorDesiredGet(&actuatorDesired);
// A flag to track which stabilization mode each axis is in
static uint8_t previous_mode[MAX_AXES] = {255,255,255};
bool error = false;
//Run the selected stabilization algorithm on each axis:
for(uint8_t i=0; i< MAX_AXES; i++)
{
// Check whether this axis mode needs to be reinitialized
bool reinit = (stabDesired.StabilizationMode[i] != previous_mode[i]);
previous_mode[i] = stabDesired.StabilizationMode[i];
// Apply the selected control law
switch(stabDesired.StabilizationMode[i])
{
case STABILIZATIONDESIRED_STABILIZATIONMODE_RATE:
if(reinit)
pids[PID_RATE_ROLL + i].iAccumulator = 0;
// Store to rate desired variable for storing to UAVO
rateDesiredAxis[i] = bound(attitudeDesiredAxis[i], settings.ManualRate[i]);
// Compute the inner loop
actuatorDesiredAxis[i] = pid_apply(&pids[PID_RATE_ROLL + i], rateDesiredAxis[i] - gyro_filtered[i], dT);
actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i],1.0f);
break;
case STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE:
if(reinit) {
pids[PID_ROLL + i].iAccumulator = 0;
pids[PID_RATE_ROLL + i].iAccumulator = 0;
}
// Compute the outer loop
rateDesiredAxis[i] = pid_apply(&pids[PID_ROLL + i], local_error[i], dT);
rateDesiredAxis[i] = bound(rateDesiredAxis[i], settings.MaximumRate[i]);
// Compute the inner loop
actuatorDesiredAxis[i] = pid_apply(&pids[PID_RATE_ROLL + i], rateDesiredAxis[i] - gyro_filtered[i], dT);
actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i],1.0f);
break;
case STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR:
// Store for debugging output
rateDesiredAxis[i] = attitudeDesiredAxis[i];
// Zero attitude and axis lock accumulators
pids[PID_ROLL + i].iAccumulator = 0;
axis_lock_accum[i] = 0;
break;
// Run a virtual flybar stabilization algorithm on this axis
stabilization_virtual_flybar(gyro_filtered[i], rateDesiredAxis[i], &actuatorDesiredAxis[i], dT, reinit, i, &settings);
break;
case STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING:
{
if (reinit)
pids[PID_RATE_ROLL + i].iAccumulator = 0;
float weak_leveling = local_error[i] * weak_leveling_kp;
weak_leveling = bound(weak_leveling, weak_leveling_max);
if(weak_leveling > weak_leveling_max)
weak_leveling = weak_leveling_max;
if(weak_leveling < -weak_leveling_max)
weak_leveling = -weak_leveling_max;
// Compute desired rate as input biased towards leveling
rateDesiredAxis[i] = attitudeDesiredAxis[i] + weak_leveling;
actuatorDesiredAxis[i] = pid_apply(&pids[PID_RATE_ROLL + i], rateDesiredAxis[i] - gyro_filtered[i], dT);
actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i],1.0f);
// Zero attitude and axis lock accumulators
pids[PID_ROLL + i].iAccumulator = 0;
axis_lock_accum[i] = 0;
break;
}
case STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE:
rateDesiredAxis[i] = ApplyPid(&pids[PID_ROLL + i], local_error[i], dT);
if(rateDesiredAxis[i] > settings.MaximumRate[i])
rateDesiredAxis[i] = settings.MaximumRate[i];
else if(rateDesiredAxis[i] < -settings.MaximumRate[i])
rateDesiredAxis[i] = -settings.MaximumRate[i];
axis_lock_accum[i] = 0;
break;
case STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK:
if (reinit)
pids[PID_RATE_ROLL + i].iAccumulator = 0;
if(fabs(attitudeDesiredAxis[i]) > max_axislock_rate) {
// While getting strong commands act like rate mode
rateDesiredAxis[i] = attitudeDesiredAxis[i];
@ -276,154 +303,96 @@ static void stabilizationTask(void* parameters)
} else {
// For weaker commands or no command simply attitude lock (almost) on no gyro change
axis_lock_accum[i] += (attitudeDesiredAxis[i] - gyro_filtered[i]) * dT;
if(axis_lock_accum[i] > max_axis_lock)
axis_lock_accum[i] = max_axis_lock;
else if(axis_lock_accum[i] < -max_axis_lock)
axis_lock_accum[i] = -max_axis_lock;
rateDesiredAxis[i] = ApplyPid(&pids[PID_ROLL + i], axis_lock_accum[i], dT);
axis_lock_accum[i] = bound(axis_lock_accum[i], max_axis_lock);
rateDesiredAxis[i] = pid_apply(&pids[PID_ROLL + i], axis_lock_accum[i], dT);
}
if(rateDesiredAxis[i] > settings.MaximumRate[i])
rateDesiredAxis[i] = settings.MaximumRate[i];
else if(rateDesiredAxis[i] < -settings.MaximumRate[i])
rateDesiredAxis[i] = -settings.MaximumRate[i];
rateDesiredAxis[i] = bound(rateDesiredAxis[i], settings.MaximumRate[i]);
actuatorDesiredAxis[i] = pid_apply(&pids[PID_RATE_ROLL + i], rateDesiredAxis[i] - gyro_filtered[i], dT);
actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i],1.0f);
break;
case STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE:
// Store to rate desired variable for storing to UAVO
rateDesiredAxis[i] = bound(attitudeDesiredAxis[i], settings.ManualRate[i]);
// Run the relay controller which also estimates the oscillation parameters
stabilization_relay_rate(rateDesiredAxis[i] - gyro_filtered[i], &actuatorDesiredAxis[i], i, reinit);
actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i],1.0);
break;
case STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE:
if(reinit)
pids[PID_ROLL + i].iAccumulator = 0;
// Compute the outer loop like attitude mode
rateDesiredAxis[i] = pid_apply(&pids[PID_ROLL + i], local_error[i], dT);
rateDesiredAxis[i] = bound(rateDesiredAxis[i], settings.MaximumRate[i]);
// Run the relay controller which also estimates the oscillation parameters
stabilization_relay_rate(rateDesiredAxis[i] - gyro_filtered[i], &actuatorDesiredAxis[i], i, reinit);
actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i],1.0);
break;
case STABILIZATIONDESIRED_STABILIZATIONMODE_NONE:
actuatorDesiredAxis[i] = bound(attitudeDesiredAxis[i],1.0f);
break;
default:
error = true;
break;
}
}
// Piro compensation rotates the virtual flybar by yaw step to keep it
// rotated to external world
if (vbar_piro_comp) {
const float F_PI = 3.14159f;
float cy = cosf(gyro_filtered[2] / 180.0f * F_PI * dT);
float sy = sinf(gyro_filtered[2] / 180.0f * F_PI * dT);
float vbar_pitch = cy * vbar_integral[1] - sy * vbar_integral[0];
float vbar_roll = sy * vbar_integral[1] + cy * vbar_integral[0];
vbar_integral[1] = vbar_pitch;
vbar_integral[0] = vbar_roll;
}
if (settings.VbarPiroComp == STABILIZATIONSETTINGS_VBARPIROCOMP_TRUE)
stabilization_virtual_flybar_pirocomp(gyro_filtered[2], dT);
uint8_t shouldUpdate = 1;
#if defined(DIAGNOSTICS)
RateDesiredSet(&rateDesired);
#endif
ActuatorDesiredGet(&actuatorDesired);
//Calculate desired command
for(uint32_t i = 0; i < MAX_AXES; i++)
{
switch(stabDesired.StabilizationMode[i])
{
case STABILIZATIONDESIRED_STABILIZATIONMODE_RATE:
case STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE:
case STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK:
case STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING:
{
float command = ApplyPid(&pids[PID_RATE_ROLL + i], rateDesiredAxis[i] - gyro_filtered[i], dT);
actuatorDesiredAxis[i] = bound(command);
break;
}
case STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR:
{
// Track the angle of the virtual flybar which includes a slow decay
vbar_integral[i] = vbar_integral[i] * vbar_decay + gyro_filtered[i] * dT;
if (vbar_integral[i] > settings.VbarMaxAngle)
vbar_integral[i] = settings.VbarMaxAngle;
if (-vbar_integral[i] < -settings.VbarMaxAngle)
vbar_integral[i] = -settings.VbarMaxAngle;
// Command signal is composed of stick input added to the gyro and virtual flybar
float gyro_gain = 1.0f;
if (vbar_gyros_suppress > 0) {
gyro_gain = (1.0f - fabs(rateDesiredAxis[i]) * vbar_gyros_suppress / 100.0f);
gyro_gain = (gyro_gain < 0) ? 0 : gyro_gain;
}
float command = rateDesiredAxis[i] * vbar_sensitivity[i] - gyro_gain * (
vbar_integral[i] * pids[PID_VBAR_ROLL + i].i +
gyro_filtered[i] * pids[PID_VBAR_ROLL + i].p);
actuatorDesiredAxis[i] = bound(command);
break;
}
case STABILIZATIONDESIRED_STABILIZATIONMODE_NONE:
switch (i)
{
case ROLL:
actuatorDesiredAxis[i] = bound(attitudeDesiredAxis[i]);
shouldUpdate = 1;
pids[PID_RATE_ROLL].iAccumulator = 0;
pids[PID_ROLL].iAccumulator = 0;
break;
case PITCH:
actuatorDesiredAxis[i] = bound(attitudeDesiredAxis[i]);
shouldUpdate = 1;
pids[PID_RATE_PITCH].iAccumulator = 0;
pids[PID_PITCH].iAccumulator = 0;
break;
case YAW:
actuatorDesiredAxis[i] = bound(attitudeDesiredAxis[i]);
shouldUpdate = 1;
pids[PID_RATE_YAW].iAccumulator = 0;
pids[PID_YAW].iAccumulator = 0;
break;
}
break;
}
}
// Save dT
actuatorDesired.UpdateTime = dT * 1000;
actuatorDesired.Throttle = stabDesired.Throttle;
if(PARSE_FLIGHT_MODE(flightStatus.FlightMode) == FLIGHTMODE_MANUAL)
shouldUpdate = 0;
if(shouldUpdate)
{
actuatorDesired.Throttle = stabDesired.Throttle;
if(dT > 15)
actuatorDesired.NumLongUpdates++;
if(PARSE_FLIGHT_MODE(flightStatus.FlightMode) != FLIGHTMODE_MANUAL) {
ActuatorDesiredSet(&actuatorDesired);
} else {
// Force all axes to reinitialize when engaged
for(uint8_t i=0; i< MAX_AXES; i++)
previous_mode[i] = 255;
}
if(flightStatus.Armed != FLIGHTSTATUS_ARMED_ARMED ||
(lowThrottleZeroIntegral && stabDesired.Throttle < 0) ||
!shouldUpdate)
(lowThrottleZeroIntegral && stabDesired.Throttle < 0))
{
ZeroPids();
// Force all axes to reinitialize when engaged
for(uint8_t i=0; i< MAX_AXES; i++)
previous_mode[i] = 255;
}
// Clear alarms
AlarmsClear(SYSTEMALARMS_ALARM_STABILIZATION);
// Clear or set alarms. Done like this to prevent toggline each cycle
// and hammering system alarms
if (error)
AlarmsSet(SYSTEMALARMS_ALARM_STABILIZATION,SYSTEMALARMS_ALARM_ERROR);
else
AlarmsClear(SYSTEMALARMS_ALARM_STABILIZATION);
}
}
float ApplyPid(pid_type * pid, const float err, float dT)
{
float diff = (err - pid->lastErr);
pid->lastErr = err;
// Scale up accumulator by 1000 while computing to avoid losing precision
pid->iAccumulator += err * (pid->i * dT * 1000.0f);
if(pid->iAccumulator > (pid->iLim * 1000.0f)) {
pid->iAccumulator = pid->iLim * 1000.0f;
} else if (pid->iAccumulator < -(pid->iLim * 1000.0f)) {
pid->iAccumulator = -pid->iLim * 1000.0f;
}
return ((err * pid->p) + pid->iAccumulator / 1000.0f + (diff * pid->d / dT));
}
/**
* Clear the accumulators and derivatives for all the axes
*/
static void ZeroPids(void)
{
for(int8_t ct = 0; ct < PID_MAX; ct++) {
pids[ct].iAccumulator = 0.0f;
pids[ct].lastErr = 0.0f;
}
for(uint32_t i = 0; i < PID_MAX; i++)
pid_zero(&pids[i]);
for(uint8_t i = 0; i < 3; i++)
axis_lock_accum[i] = 0.0f;
}
@ -432,12 +401,12 @@ static void ZeroPids(void)
/**
* Bound input value between limits
*/
static float bound(float val)
static float bound(float val, float range)
{
if(val < -1.0f) {
val = -1.0f;
} else if(val > 1.0f) {
val = 1.0f;
if(val < -range) {
val = -range;
} else if(val > range) {
val = range;
}
return val;
}
@ -445,68 +414,51 @@ static float bound(float val)
static void SettingsUpdatedCb(UAVObjEvent * ev)
{
StabilizationSettingsGet(&settings);
// Set the roll rate PID constants
pids[PID_RATE_ROLL].p = settings.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KP];
pids[PID_RATE_ROLL].i = settings.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KI];
pids[PID_RATE_ROLL].d = settings.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KD];
pids[PID_RATE_ROLL].iLim = settings.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_ILIMIT];
pid_configure(&pids[PID_RATE_ROLL], settings.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KP],
settings.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KI],
pids[PID_RATE_ROLL].d = settings.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KD],
pids[PID_RATE_ROLL].iLim = settings.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_ILIMIT]);
// Set the pitch rate PID constants
pids[PID_RATE_PITCH].p = settings.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KP];
pids[PID_RATE_PITCH].i = settings.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KI];
pids[PID_RATE_PITCH].d = settings.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KD];
pids[PID_RATE_PITCH].iLim = settings.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_ILIMIT];
pid_configure(&pids[PID_RATE_PITCH], settings.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KP],
pids[PID_RATE_PITCH].i = settings.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KI],
pids[PID_RATE_PITCH].d = settings.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KD],
pids[PID_RATE_PITCH].iLim = settings.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_ILIMIT]);
// Set the yaw rate PID constants
pids[PID_RATE_YAW].p = settings.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_KP];
pids[PID_RATE_YAW].i = settings.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_KI];
pids[PID_RATE_YAW].d = settings.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_KD];
pids[PID_RATE_YAW].iLim = settings.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_ILIMIT];
pid_configure(&pids[PID_RATE_YAW], settings.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_KP],
pids[PID_RATE_YAW].i = settings.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_KI],
pids[PID_RATE_YAW].d = settings.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_KD],
pids[PID_RATE_YAW].iLim = settings.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_ILIMIT]);
// Set the roll attitude PI constants
pids[PID_ROLL].p = settings.RollPI[STABILIZATIONSETTINGS_ROLLPI_KP];
pids[PID_ROLL].i = settings.RollPI[STABILIZATIONSETTINGS_ROLLPI_KI];
pids[PID_ROLL].iLim = settings.RollPI[STABILIZATIONSETTINGS_ROLLPI_ILIMIT];
// Set the pitch attitude PI constants
pids[PID_PITCH].p = settings.PitchPI[STABILIZATIONSETTINGS_PITCHPI_KP];
pids[PID_PITCH].i = settings.PitchPI[STABILIZATIONSETTINGS_PITCHPI_KI];
pids[PID_PITCH].iLim = settings.PitchPI[STABILIZATIONSETTINGS_PITCHPI_ILIMIT];
// Set the yaw attitude PI constants
pids[PID_YAW].p = settings.YawPI[STABILIZATIONSETTINGS_YAWPI_KP];
pids[PID_YAW].i = settings.YawPI[STABILIZATIONSETTINGS_YAWPI_KI];
pids[PID_YAW].iLim = settings.YawPI[STABILIZATIONSETTINGS_YAWPI_ILIMIT];
// Set the roll attitude PI constants
pids[PID_VBAR_ROLL].p = settings.VbarRollPI[STABILIZATIONSETTINGS_VBARROLLPI_KP];
pids[PID_VBAR_ROLL].i = settings.VbarRollPI[STABILIZATIONSETTINGS_VBARROLLPI_KI];
pid_configure(&pids[PID_ROLL], settings.RollPI[STABILIZATIONSETTINGS_ROLLPI_KP],
settings.RollPI[STABILIZATIONSETTINGS_ROLLPI_KI], 0,
pids[PID_ROLL].iLim = settings.RollPI[STABILIZATIONSETTINGS_ROLLPI_ILIMIT]);
// Set the pitch attitude PI constants
pids[PID_VBAR_PITCH].p = settings.VbarPitchPI[STABILIZATIONSETTINGS_VBARPITCHPI_KP];
pids[PID_VBAR_PITCH].i = settings.VbarPitchPI[STABILIZATIONSETTINGS_VBARPITCHPI_KI];
pid_configure(&pids[PID_PITCH], settings.PitchPI[STABILIZATIONSETTINGS_PITCHPI_KP],
pids[PID_PITCH].i = settings.PitchPI[STABILIZATIONSETTINGS_PITCHPI_KI], 0,
settings.PitchPI[STABILIZATIONSETTINGS_PITCHPI_ILIMIT]);
// Set the yaw attitude PI constants
pids[PID_VBAR_YAW].p = settings.VbarYawPI[STABILIZATIONSETTINGS_VBARYAWPI_KP];
pids[PID_VBAR_YAW].i = settings.VbarYawPI[STABILIZATIONSETTINGS_VBARYAWPI_KI];
// Need to store the vbar sensitivity
vbar_sensitivity[0] = settings.VbarSensitivity[0];
vbar_sensitivity[1] = settings.VbarSensitivity[1];
vbar_sensitivity[2] = settings.VbarSensitivity[2];
pid_configure(&pids[PID_YAW], settings.YawPI[STABILIZATIONSETTINGS_YAWPI_KP],
settings.YawPI[STABILIZATIONSETTINGS_YAWPI_KI], 0,
settings.YawPI[STABILIZATIONSETTINGS_YAWPI_ILIMIT]);
// Maximum deviation to accumulate for axis lock
max_axis_lock = settings.MaxAxisLock;
max_axislock_rate = settings.MaxAxisLockRate;
// Settings for weak leveling
weak_leveling_kp = settings.WeakLevelingKp;
weak_leveling_max = settings.MaxWeakLevelingRate;
// Whether to zero the PID integrals while throttle is low
lowThrottleZeroIntegral = settings.LowThrottleZeroIntegral == STABILIZATIONSETTINGS_LOWTHROTTLEZEROINTEGRAL_TRUE;
// The dT has some jitter iteration to iteration that we don't want to
// make thie result unpredictable. Still, it's nicer to specify the constant
// based on a time (in ms) rather than a fixed multiplier. The error between
@ -517,11 +469,9 @@ static void SettingsUpdatedCb(UAVObjEvent * ev)
gyro_alpha = 0; // not trusting this to resolve to 0
else
gyro_alpha = expf(-fakeDt / settings.GyroTau);
// Compute time constant for vbar decay term based on a tau
vbar_decay = expf(-fakeDt / settings.VbarTau);
vbar_gyros_suppress = settings.VbarGyroSuppress;
vbar_piro_comp = settings.VbarPiroComp == STABILIZATIONSETTINGS_VBARPIROCOMP_TRUE;
}
@ -529,3 +479,4 @@ static void SettingsUpdatedCb(UAVObjEvent * ev)
* @}
* @}
*/

119
flight/Modules/Stabilization/virtualflybar.c Normal file
View File

@ -0,0 +1,119 @@
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup StabilizationModule Stabilization Module
* @brief Virtual flybar mode
* @note This file implements the logic for a virtual flybar
* @{
*
* @file virtualflybar.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
* @brief Attitude stabilization module.
*
* @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
*/
#include "openpilot.h"
#include "stabilization.h"
#include "stabilizationsettings.h"
//! Private variables
static float vbar_integral[MAX_AXES];
static float vbar_decay = 0.991f;
//! Private methods
static float bound(float val, float range);
int stabilization_virtual_flybar(float gyro, float command, float *output, float dT, bool reinit, uint32_t axis, StabilizationSettingsData *settings)
{
float gyro_gain = 1.0f;
float kp = 0, ki = 0;
if(reinit)
vbar_integral[axis] = 0;
// Track the angle of the virtual flybar which includes a slow decay
vbar_integral[axis] = vbar_integral[axis] * vbar_decay + gyro * dT;
vbar_integral[axis] = bound(vbar_integral[axis], settings->VbarMaxAngle);
// Command signal can indicate how much to disregard the gyro feedback (fast flips)
if (settings->VbarGyroSuppress > 0) {
gyro_gain = (1.0f - fabs(command) * settings->VbarGyroSuppress / 100.0f);
gyro_gain = (gyro_gain < 0) ? 0 : gyro_gain;
}
// Get the settings for the correct axis
switch(axis)
{
case ROLL:
kp = settings->VbarRollPI[STABILIZATIONSETTINGS_VBARROLLPI_KP];
ki = settings->VbarRollPI[STABILIZATIONSETTINGS_VBARROLLPI_KI];
break;
case PITCH:
kp = settings->VbarPitchPI[STABILIZATIONSETTINGS_VBARROLLPI_KP];
ki = settings->VbarPitchPI[STABILIZATIONSETTINGS_VBARROLLPI_KI];
break;
case YAW:
kp = settings->VbarYawPI[STABILIZATIONSETTINGS_VBARROLLPI_KP];
ki = settings->VbarYawPI[STABILIZATIONSETTINGS_VBARROLLPI_KI];
break;
default:
PIOS_DEBUG_Assert(0);
}
// Command signal is composed of stick input added to the gyro and virtual flybar
*output = command * settings->VbarSensitivity[axis] -
gyro_gain * (vbar_integral[axis] * ki + gyro * kp);
return 0;
}
/**
* Want to keep the virtual flybar fixed in world coordinates as we pirouette
* @param[in] z_gyro The deg/s of rotation along the z axis
* @param[in] dT The time since last sample
*/
int stabilization_virtual_flybar_pirocomp(float z_gyro, float dT)
{
const float F_PI = (float) M_PI;
float cy = cosf(z_gyro / 180.0f * F_PI * dT);
float sy = sinf(z_gyro / 180.0f * F_PI * dT);
float vbar_pitch = cy * vbar_integral[1] - sy * vbar_integral[0];
float vbar_roll = sy * vbar_integral[1] + cy * vbar_integral[0];
vbar_integral[1] = vbar_pitch;
vbar_integral[0] = vbar_roll;
return 0;
}
/**
* Bound input value between limits
*/
static float bound(float val, float range)
{
if(val < -range) {
val = -range;
} else if(val > range) {
val = range;
}
return val;
}

1
flight/PiOS/Boards/STM32103CB_CC_Rev1.h
View File

@ -74,6 +74,7 @@ TIM4 | RC In 1 | Servo 3 | Servo 2 | Servo 1
#define PIOS_WDG_STABILIZATION 0x0002
#define PIOS_WDG_ATTITUDE 0x0004
#define PIOS_WDG_MANUAL 0x0008
#define PIOS_WDG_AUTOTUNE 0x0010
//------------------------
// TELEMETRY

5
flight/RevoMini/Makefile
View File

@ -75,6 +75,8 @@ PIOSINC = $(PIOS)/inc
OPMODULEDIR = ../Modules
FLIGHTLIB = ../Libraries
FLIGHTLIBINC = ../Libraries/inc
MATHLIB = ../Libraries/math
MATHLIBINC = ../Libraries/math
RSCODE = $(FLIGHTLIB)/rscode
RSCODEINC = $(FLIGHTLIB)/rscode
PIOSSTM32F4XX = $(PIOS)/STM32F4xx
@ -144,6 +146,8 @@ SRC += $(FLIGHTLIB)/fifo_buffer.c
SRC += $(FLIGHTLIB)/WorldMagModel.c
SRC += $(FLIGHTLIB)/insgps13state.c
SRC += $(FLIGHTLIB)/taskmonitor.c
SRC += $(MATHLIB)/sin_lookup.c
SRC += $(MATHLIB)/pid.c
## For RFM22b
SRC += $(FLIGHTLIB)/packet_handler.c
@ -214,6 +218,7 @@ EXTRAINCDIRS += $(OPUAVOBJ)
EXTRAINCDIRS += $(OPUAVOBJINC)
EXTRAINCDIRS += $(UAVOBJSYNTHDIR)
EXTRAINCDIRS += $(FLIGHTLIBINC)
EXTRAINCDIRS += $(MATHLIBINC)
EXTRAINCDIRS += $(RSCODEINC)
EXTRAINCDIRS += $(PIOSSTM32F4XX)
EXTRAINCDIRS += $(PIOSCOMMON)

2
flight/RevoMini/UAVObjects.inc
View File

@ -72,6 +72,8 @@ UAVOBJSRCFILENAMES += pipxstatus
UAVOBJSRCFILENAMES += pipxsettings
UAVOBJSRCFILENAMES += positionactual
UAVOBJSRCFILENAMES += ratedesired
UAVOBJSRCFILENAMES += relaytuning
UAVOBJSRCFILENAMES += relaytuningsettings
UAVOBJSRCFILENAMES += revocalibration
UAVOBJSRCFILENAMES += revosettings
UAVOBJSRCFILENAMES += sonaraltitude

2
flight/Revolution/UAVObjects.inc
View File

@ -70,6 +70,8 @@ UAVOBJSRCFILENAMES += pathplannersettings
UAVOBJSRCFILENAMES += pathdesired
UAVOBJSRCFILENAMES += positionactual
UAVOBJSRCFILENAMES += ratedesired
UAVOBJSRCFILENAMES += relaytuning
UAVOBJSRCFILENAMES += relaytuningsettings
UAVOBJSRCFILENAMES += revocalibration
UAVOBJSRCFILENAMES += revosettings
UAVOBJSRCFILENAMES += sonaraltitude

1084
ground/openpilotgcs/src/plugins/config/autotune.ui Normal file
View File

@ -0,0 +1,1084 @@
<?xml version="1.0" encoding="UTF-8"?>
<ui version="4.0">
<class>AutotuneWidget</class>
<widget class="QWidget" name="AutotuneWidget">
<property name="geometry">
<rect>
<x>0</x>
<y>0</y>
<width>443</width>
<height>575</height>
</rect>
</property>
<property name="windowTitle">
<string>Form</string>
</property>
<widget class="QGroupBox" name="groupBox">
<property name="geometry">
<rect>
<x>20</x>
<y>10</y>
<width>401</width>
<height>131</height>
</rect>
</property>
<property name="title">
<string>Tuning Aggressiveness</string>
</property>
<layout class="QGridLayout" name="gridLayout_2">
<item row="0" column="0">
<widget class="QLabel" name="label">
<property name="text">
<string>Rate Tuning:</string>
</property>
</widget>
</item>
<item row="1" column="0">
<widget class="QLabel" name="label_2">
<property name="text">
<string>Attitude Tuning:</string>
</property>
</widget>
</item>
<item row="0" column="1">
<widget class="QSlider" name="rateTuning">
<property name="maximum">
<number>100</number>
</property>
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
<property name="objrelation" stdset="0">
<stringlist>
<string>objname:RelayTuningSettings</string>
<string>fieldname:RateGain</string>
<string>scale:0.01</string>
<string>haslimits:no</string>
</stringlist>
</property>
</widget>
</item>
<item row="1" column="1">
<widget class="QSlider" name="attitudeTuning">
<property name="maximum">
<number>100</number>
</property>
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
<property name="objrelation" stdset="0">
<stringlist>
<string>objname:RelayTuningSettings</string>
<string>fieldname:AttitudeGain</string>
<string>scale:0.01</string>
<string>haslimits:no</string>
</stringlist>
</property>
</widget>
</item>
</layout>
</widget>
<widget class="QGroupBox" name="groupBox_2">
<property name="geometry">
<rect>
<x>20</x>
<y>250</y>
<width>401</width>
<height>121</height>
</rect>
</property>
<property name="title">
<string>Computed Values</string>
</property>
<layout class="QGridLayout" name="gridLayout_3">
<item row="0" column="1">
<widget class="QLabel" name="label_5">
<property name="text">
<string>RateKp</string>
</property>
</widget>
</item>
<item row="1" column="0">
<widget class="QLabel" name="label_4">
<property name="text">
<string>Roll</string>
</property>
</widget>
</item>
<item row="0" column="2">
<widget class="QLabel" name="label_9">
<property name="text">
<string>RateKi</string>
</property>
</widget>
</item>
<item row="0" column="3">
<widget class="QLabel" name="label_10">
<property name="text">
<string>AttitudeKp</string>
</property>
</widget>
</item>
<item row="0" column="4">
<widget class="QLabel" name="label_11">
<property name="text">
<string>AttitudeKi</string>
</property>
</widget>
</item>
<item row="2" column="0">
<widget class="QLabel" name="label_12">
<property name="text">
<string>Pitch</string>
</property>
</widget>
</item>
<item row="1" column="1">
<widget class="QLabel" name="rollRateKp">
<property name="text">
<string>0</string>
</property>
</widget>
</item>
<item row="2" column="1">
<widget class="QLabel" name="pitchRateKp">
<property name="text">
<string>0</string>
</property>
</widget>
</item>
<item row="1" column="2">
<widget class="QLabel" name="rollRateKi">
<property name="text">
<string>0</string>
</property>
</widget>
</item>
<item row="2" column="2">
<widget class="QLabel" name="pitchRateKi">
<property name="text">
<string>0</string>
</property>
</widget>
</item>
<item row="1" column="3">
<widget class="QLabel" name="rollAttitudeKp">
<property name="text">
<string>0</string>
</property>
</widget>
</item>
<item row="2" column="3">
<widget class="QLabel" name="pitchAttitudeKp">
<property name="text">
<string>0</string>
</property>
</widget>
</item>
<item row="1" column="4">
<widget class="QLabel" name="rollAttitudeKi">
<property name="text">
<string>0</string>
</property>
</widget>
</item>
<item row="2" column="4">
<widget class="QLabel" name="pitchAttitudeKi">
<property name="text">
<string>0</string>
</property>
</widget>
</item>
</layout>
</widget>
<widget class="QGroupBox" name="groupBox_3">
<property name="geometry">
<rect>
<x>20</x>
<y>140</y>
<width>401</width>
<height>111</height>
</rect>
</property>
<property name="title">
<string>Measured Properties</string>
</property>
<layout class="QGridLayout" name="gridLayout">
<item row="1" column="0">
<widget class="QLabel" name="label_3">
<property name="text">
<string>Roll:</string>
</property>
</widget>
</item>
<item row="1" column="1">
<widget class="QLabel" name="measuredRollPeriod">
<property name="text">
<string>0</string>
</property>
<property name="alignment">
<set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
</property>
<property name="objrelation" stdset="0">
<stringlist>
<string>objname:RelayTuning</string>
<string>fieldname:Period</string>
<string>element:Roll</string>
</stringlist>
</property>
</widget>
</item>
<item row="1" column="2">
<widget class="QLabel" name="measuredRollGain">
<property name="text">
<string>0</string>
</property>
<property name="alignment">
<set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
</property>
<property name="objrelation" stdset="0">
<stringlist>
<string>objname:RelayTuning</string>
<string>fieldname:Gain</string>
<string>element:Roll</string>
</stringlist>
</property>
</widget>
</item>
<item row="0" column="1">
<widget class="QLabel" name="label_6">
<property name="text">
<string>Period (ms)</string>
</property>
</widget>
</item>
<item row="0" column="2">
<widget class="QLabel" name="label_7">
<property name="text">
<string>Gain (deg/s) / output</string>
</property>
</widget>
</item>
<item row="2" column="0">
<widget class="QLabel" name="label_8">
<property name="text">
<string>Pitch</string>
</property>
</widget>
</item>
<item row="2" column="1">
<widget class="QLabel" name="measuredPitchPeriod">
<property name="text">
<string>0</string>
</property>
<property name="alignment">
<set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
</property>
<property name="objrelation" stdset="0">
<stringlist>
<string>objname:RelayTuning</string>
<string>fieldname:Period</string>
<string>element:Pitch</string>
</stringlist>
</property>
</widget>
</item>
<item row="2" column="2">
<widget class="QLabel" name="measuredPitchGain">
<property name="text">
<string>0</string>
</property>
<property name="alignment">
<set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
</property>
<property name="objrelation" stdset="0">
<stringlist>
<string>objname:RelayTuning</string>
<string>fieldname:Gain</string>
<string>element:Pitch</string>
</stringlist>
</property>
</widget>
</item>
</layout>
</widget>
<widget class="QGroupBox" name="RateStabilizationGroup_21">
<property name="geometry">
<rect>
<x>20</x>
<y>480</y>
<width>401</width>
<height>79</height>
</rect>
</property>
<property name="sizePolicy">
<sizepolicy hsizetype="Preferred" vsizetype="Fixed">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="minimumSize">
<size>
<width>0</width>
<height>79</height>
</size>
</property>
<property name="maximumSize">
<size>
<width>16777215</width>
<height>79</height>
</size>
</property>
<property name="palette">
<palette>
<active>
<colorrole role="WindowText">
<brush brushstyle="SolidPattern">
<color alpha="255">
<red>0</red>
<green>0</green>
<blue>0</blue>
</color>
</brush>
</colorrole>
<colorrole role="Button">
<brush brushstyle="LinearGradientPattern">
<gradient startx="0.507000000000000" starty="0.869318000000000" endx="0.507000000000000" endy="0.096590900000000" type="LinearGradient" spread="PadSpread" coordinatemode="ObjectBoundingMode">
<gradientstop position="0.000000000000000">
<color alpha="255">
<red>243</red>
<green>243</green>
<blue>243</blue>
</color>
</gradientstop>
<gradientstop position="1.000000000000000">
<color alpha="255">
<red>250</red>
<green>250</green>
<blue>250</blue>
</color>
</gradientstop>
</gradient>
</brush>
</colorrole>
<colorrole role="Light">
<brush brushstyle="SolidPattern">
<color alpha="255">
<red>255</red>
<green>255</green>
<blue>255</blue>
</color>
</brush>
</colorrole>
<colorrole role="Midlight">
<brush brushstyle="SolidPattern">
<color alpha="255">
<red>251</red>
<green>251</green>
<blue>251</blue>
</color>
</brush>
</colorrole>
<colorrole role="Dark">
<brush brushstyle="SolidPattern">
<color alpha="255">
<red>124</red>
<green>124</green>
<blue>124</blue>
</color>
</brush>
</colorrole>
<colorrole role="Mid">
<brush brushstyle="SolidPattern">
<color alpha="255">
<red>165</red>
<green>165</green>
<blue>165</blue>
</color>
</brush>
</colorrole>
<colorrole role="Text">
<brush brushstyle="SolidPattern">
<color alpha="255">
<red>0</red>
<green>0</green>
<blue>0</blue>
</color>
</brush>
</colorrole>
<colorrole role="BrightText">
<brush brushstyle="SolidPattern">
<color alpha="255">
<red>255</red>
<green>255</green>
<blue>255</blue>
</color>
</brush>
</colorrole>
<colorrole role="ButtonText">
<brush brushstyle="SolidPattern">
<color alpha="255">
<red>0</red>
<green>0</green>
<blue>0</blue>
</color>
</brush>
</colorrole>
<colorrole role="Base">
<brush brushstyle="LinearGradientPattern">
<gradient startx="0.507000000000000" starty="0.869318000000000" endx="0.507000000000000" endy="0.096590900000000" type="LinearGradient" spread="PadSpread" coordinatemode="ObjectBoundingMode">
<gradientstop position="0.000000000000000">
<color alpha="255">
<red>243</red>
<green>243</green>
<blue>243</blue>
</color>
</gradientstop>
<gradientstop position="1.000000000000000">
<color alpha="255">
<red>250</red>
<green>250</green>
<blue>250</blue>
</color>
</gradientstop>
</gradient>
</brush>
</colorrole>
<colorrole role="Window">
<brush brushstyle="LinearGradientPattern">
<gradient startx="0.507000000000000" starty="0.869318000000000" endx="0.507000000000000" endy="0.096590900000000" type="LinearGradient" spread="PadSpread" coordinatemode="ObjectBoundingMode">
<gradientstop position="0.000000000000000">
<color alpha="255">
<red>243</red>
<green>243</green>
<blue>243</blue>
</color>
</gradientstop>
<gradientstop position="1.000000000000000">
<color alpha="255">
<red>250</red>
<green>250</green>
<blue>250</blue>
</color>
</gradientstop>
</gradient>
</brush>
</colorrole>
<colorrole role="Shadow">
<brush brushstyle="SolidPattern">
<color alpha="255">
<red>0</red>
<green>0</green>
<blue>0</blue>
</color>
</brush>
</colorrole>
<colorrole role="AlternateBase">
<brush brushstyle="SolidPattern">
<color alpha="255">
<red>251</red>
<green>251</green>
<blue>251</blue>
</color>
</brush>
</colorrole>
<colorrole role="ToolTipBase">
<brush brushstyle="SolidPattern">
<color alpha="255">
<red>255</red>
<green>255</green>
<blue>220</blue>
</color>
</brush>
</colorrole>
<colorrole role="ToolTipText">
<brush brushstyle="SolidPattern">
<color alpha="255">
<red>0</red>
<green>0</green>
<blue>0</blue>
</color>
</brush>
</colorrole>
</active>
<inactive>
<colorrole role="WindowText">
<brush brushstyle="SolidPattern">
<color alpha="255">
<red>0</red>
<green>0</green>
<blue>0</blue>
</color>
</brush>
</colorrole>
<colorrole role="Button">
<brush brushstyle="LinearGradientPattern">
<gradient startx="0.507000000000000" starty="0.869318000000000" endx="0.507000000000000" endy="0.096590900000000" type="LinearGradient" spread="PadSpread" coordinatemode="ObjectBoundingMode">
<gradientstop position="0.000000000000000">
<color alpha="255">
<red>243</red>
<green>243</green>
<blue>243</blue>
</color>
</gradientstop>
<gradientstop position="1.000000000000000">
<color alpha="255">
<red>250</red>
<green>250</green>
<blue>250</blue>
</color>
</gradientstop>
</gradient>
</brush>
</colorrole>
<colorrole role="Light">
<brush brushstyle="SolidPattern">
<color alpha="255">
<red>255</red>
<green>255</green>
<blue>255</blue>
</color>
</brush>
</colorrole>
<colorrole role="Midlight">
<brush brushstyle="SolidPattern">
<color alpha="255">
<red>251</red>
<green>251</green>
<blue>251</blue>
</color>
</brush>
</colorrole>
<colorrole role="Dark">
<brush brushstyle="SolidPattern">
<color alpha="255">
<red>124</red>
<green>124</green>
<blue>124</blue>
</color>
</brush>
</colorrole>
<colorrole role="Mid">
<brush brushstyle="SolidPattern">
<color alpha="255">
<red>165</red>
<green>165</green>
<blue>165</blue>
</color>
</brush>
</colorrole>
<colorrole role="Text">
<brush brushstyle="SolidPattern">
<color alpha="255">
<red>0</red>
<green>0</green>
<blue>0</blue>
</color>
</brush>
</colorrole>
<colorrole role="BrightText">
<brush brushstyle="SolidPattern">
<color alpha="255">
<red>255</red>
<green>255</green>
<blue>255</blue>
</color>
</brush>
</colorrole>
<colorrole role="ButtonText">
<brush brushstyle="SolidPattern">
<color alpha="255">
<red>0</red>
<green>0</green>
<blue>0</blue>
</color>
</brush>
</colorrole>
<colorrole role="Base">
<brush brushstyle="LinearGradientPattern">
<gradient startx="0.507000000000000" starty="0.869318000000000" endx="0.507000000000000" endy="0.096590900000000" type="LinearGradient" spread="PadSpread" coordinatemode="ObjectBoundingMode">
<gradientstop position="0.000000000000000">
<color alpha="255">
<red>243</red>
<green>243</green>
<blue>243</blue>
</color>
</gradientstop>
<gradientstop position="1.000000000000000">
<color alpha="255">
<red>250</red>
<green>250</green>
<blue>250</blue>
</color>
</gradientstop>
</gradient>
</brush>
</colorrole>
<colorrole role="Window">
<brush brushstyle="LinearGradientPattern">
<gradient startx="0.507000000000000" starty="0.869318000000000" endx="0.507000000000000" endy="0.096590900000000" type="LinearGradient" spread="PadSpread" coordinatemode="ObjectBoundingMode">
<gradientstop position="0.000000000000000">
<color alpha="255">
<red>243</red>
<green>243</green>
<blue>243</blue>
</color>
</gradientstop>
<gradientstop position="1.000000000000000">
<color alpha="255">
<red>250</red>
<green>250</green>
<blue>250</blue>
</color>
</gradientstop>
</gradient>
</brush>
</colorrole>
<colorrole role="Shadow">
<brush brushstyle="SolidPattern">
<color alpha="255">
<red>0</red>
<green>0</green>
<blue>0</blue>
</color>
</brush>
</colorrole>
<colorrole role="AlternateBase">
<brush brushstyle="SolidPattern">
<color alpha="255">
<red>251</red>
<green>251</green>
<blue>251</blue>
</color>
</brush>
</colorrole>
<colorrole role="ToolTipBase">
<brush brushstyle="SolidPattern">
<color alpha="255">
<red>255</red>
<green>255</green>
<blue>220</blue>
</color>
</brush>
</colorrole>
<colorrole role="ToolTipText">
<brush brushstyle="SolidPattern">
<color alpha="255">
<red>0</red>
<green>0</green>
<blue>0</blue>
</color>
</brush>
</colorrole>
</inactive>
<disabled>
<colorrole role="WindowText">
<brush brushstyle="SolidPattern">
<color alpha="255">
<red>124</red>
<green>124</green>
<blue>124</blue>
</color>
</brush>
</colorrole>
<colorrole role="Button">
<brush brushstyle="LinearGradientPattern">
<gradient startx="0.507000000000000" starty="0.869318000000000" endx="0.507000000000000" endy="0.096590900000000" type="LinearGradient" spread="PadSpread" coordinatemode="ObjectBoundingMode">
<gradientstop position="0.000000000000000">
<color alpha="255">
<red>243</red>
<green>243</green>
<blue>243</blue>
</color>
</gradientstop>
<gradientstop position="1.000000000000000">
<color alpha="255">
<red>250</red>
<green>250</green>
<blue>250</blue>
</color>
</gradientstop>
</gradient>
</brush>
</colorrole>
<colorrole role="Light">
<brush brushstyle="SolidPattern">
<color alpha="255">
<red>255</red>
<green>255</green>
<blue>255</blue>
</color>
</brush>
</colorrole>
<colorrole role="Midlight">
<brush brushstyle="SolidPattern">
<color alpha="255">
<red>251</red>
<green>251</green>
<blue>251</blue>
</color>
</brush>
</colorrole>
<colorrole role="Dark">
<brush brushstyle="SolidPattern">
<color alpha="255">
<red>124</red>
<green>124</green>
<blue>124</blue>
</color>
</brush>
</colorrole>
<colorrole role="Mid">
<brush brushstyle="SolidPattern">
<color alpha="255">
<red>165</red>
<green>165</green>
<blue>165</blue>
</color>
</brush>
</colorrole>
<colorrole role="Text">
<brush brushstyle="SolidPattern">
<color alpha="255">
<red>124</red>
<green>124</green>
<blue>124</blue>
</color>
</brush>
</colorrole>
<colorrole role="BrightText">
<brush brushstyle="SolidPattern">
<color alpha="255">
<red>255</red>
<green>255</green>
<blue>255</blue>
</color>
</brush>
</colorrole>
<colorrole role="ButtonText">
<brush brushstyle="SolidPattern">
<color alpha="255">
<red>124</red>
<green>124</green>
<blue>124</blue>
</color>
</brush>
</colorrole>
<colorrole role="Base">
<brush brushstyle="LinearGradientPattern">
<gradient startx="0.507000000000000" starty="0.869318000000000" endx="0.507000000000000" endy="0.096590900000000" type="LinearGradient" spread="PadSpread" coordinatemode="ObjectBoundingMode">
<gradientstop position="0.000000000000000">
<color alpha="255">
<red>243</red>
<green>243</green>
<blue>243</blue>
</color>
</gradientstop>
<gradientstop position="1.000000000000000">
<color alpha="255">
<red>250</red>
<green>250</green>
<blue>250</blue>
</color>
</gradientstop>
</gradient>
</brush>
</colorrole>
<colorrole role="Window">
<brush brushstyle="LinearGradientPattern">
<gradient startx="0.507000000000000" starty="0.869318000000000" endx="0.507000000000000" endy="0.096590900000000" type="LinearGradient" spread="PadSpread" coordinatemode="ObjectBoundingMode">
<gradientstop position="0.000000000000000">
<color alpha="255">
<red>243</red>
<green>243</green>
<blue>243</blue>
</color>
</gradientstop>
<gradientstop position="1.000000000000000">
<color alpha="255">
<red>250</red>
<green>250</green>
<blue>250</blue>
</color>
</gradientstop>
</gradient>
</brush>
</colorrole>
<colorrole role="Shadow">
<brush brushstyle="SolidPattern">
<color alpha="255">
<red>0</red>
<green>0</green>
<blue>0</blue>
</color>
</brush>
</colorrole>
<colorrole role="AlternateBase">
<brush brushstyle="SolidPattern">
<color alpha="255">
<red>248</red>
<green>248</green>
<blue>248</blue>
</color>
</brush>
</colorrole>
<colorrole role="ToolTipBase">
<brush brushstyle="SolidPattern">
<color alpha="255">
<red>255</red>
<green>255</green>
<blue>220</blue>
</color>
</brush>
</colorrole>
<colorrole role="ToolTipText">
<brush brushstyle="SolidPattern">
<color alpha="255">
<red>0</red>
<green>0</green>
<blue>0</blue>
</color>
</brush>
</colorrole>
</disabled>
</palette>
</property>
<property name="autoFillBackground">
<bool>false</bool>
</property>
<property name="title">
<string/>
</property>
<property name="alignment">
<set>Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter</set>
</property>
<layout class="QGridLayout" name="gridLayout_10">
<item row="0" column="0">
<spacer name="horizontalSpacer_3">
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>111</width>
<height>10</height>
</size>
</property>
</spacer>
</item>
<item row="0" column="1">
<widget class="QPushButton" name="stabilizationReloadBoardData_6">
<property name="minimumSize">
<size>
<width>120</width>
<height>28</height>
</size>
</property>
<property name="toolTip">
<string>Reloads the saved settings into GCS.
Useful if you have accidentally changed some settings.</string>
</property>
<property name="styleSheet">
<string notr="true">QPushButton {
border: 1px outset #999;
border-radius: 5;
background-color: qlineargradient(spread:pad, x1:0.507, y1:0.869318, x2:0.507, y2:0.0965909, stop:0.28 rgba(236, 236, 236, 255), stop:1 rgba(252, 252, 252, 255));
}
QPushButton:pressed {
border-style: inset;
background-color: qlineargradient(spread:pad, x1:0.502, y1:0.664864, x2:0.502, y2:0.034, stop:0.358209 rgba(250, 250, 250, 255), stop:0.626866 rgba(235, 235, 235, 255));
}
QPushButton:hover {
border: 1px outset #999;
border-color: rgb(83, 83, 83);
border-radius: 4;
}</string>
</property>
<property name="text">
<string>Reload Board Data</string>
</property>
<property name="objrelation" stdset="0">
<stringlist>
<string>button:reload</string>
<string>buttongroup:10</string>
</stringlist>
</property>
</widget>
</item>
<item row="0" column="2">
<widget class="QPushButton" name="saveStabilizationToRAM_6">
<property name="minimumSize">
<size>
<width>60</width>
<height>28</height>
</size>
</property>
<property name="toolTip">
<string>Send settings to the board but do not save to the non-volatile memory</string>
</property>
<property name="styleSheet">
<string notr="true">QPushButton {
border: 1px outset #999;
border-radius: 5;
background-color: qlineargradient(spread:pad, x1:0.507, y1:0.869318, x2:0.507, y2:0.0965909, stop:0.28 rgba(236, 236, 236, 255), stop:1 rgba(252, 252, 252, 255));
}
QPushButton:pressed {
border-style: inset;
background-color: qlineargradient(spread:pad, x1:0.502, y1:0.664864, x2:0.502, y2:0.034, stop:0.358209 rgba(250, 250, 250, 255), stop:0.626866 rgba(235, 235, 235, 255));
}
QPushButton:hover {
border: 1px outset #999;
border-color: rgb(83, 83, 83);
border-radius: 4;
}</string>
</property>
<property name="text">
<string>Apply</string>
</property>
<property name="objrelation" stdset="0">
<stringlist>
<string>button:apply</string>
</stringlist>
</property>
</widget>
</item>
<item row="0" column="3">
<widget class="QPushButton" name="saveStabilizationToSD_6">
<property name="minimumSize">
<size>
<width>60</width>
<height>28</height>
</size>
</property>
<property name="toolTip">
<string>Send settings to the board and save to the non-volatile memory</string>
</property>
<property name="styleSheet">
<string notr="true">QPushButton {
border: 1px outset #999;
border-radius: 5;
background-color: qlineargradient(spread:pad, x1:0.507, y1:0.869318, x2:0.507, y2:0.0965909, stop:0.28 rgba(236, 236, 236, 255), stop:1 rgba(252, 252, 252, 255));
}
QPushButton:pressed {
border-style: inset;
background-color: qlineargradient(spread:pad, x1:0.502, y1:0.664864, x2:0.502, y2:0.034, stop:0.358209 rgba(250, 250, 250, 255), stop:0.626866 rgba(235, 235, 235, 255));
}
QPushButton:hover {
border: 1px outset #999;
border-color: rgb(83, 83, 83);
border-radius: 4;
}</string>
</property>
<property name="text">
<string>Save</string>
</property>
<property name="objrelation" stdset="0">
<stringlist>
<string>button:save</string>
</stringlist>
</property>
</widget>
</item>
</layout>
</widget>
<widget class="QLabel" name="label_22">
<property name="geometry">
<rect>
<x>30</x>
<y>440</y>
<width>371</width>
<height>31</height>
</rect>
</property>
<property name="text">
<string>The buttons below change the autotuning settings which
will alter thingsfor the next autotuning flight</string>
</property>
</widget>
<widget class="QPushButton" name="useComputedValues">
<property name="geometry">
<rect>
<x>250</x>
<y>380</y>
<width>171</width>
<height>31</height>
</rect>
</property>
<property name="styleSheet">
<string notr="true">QPushButton {
border: 1px outset #999;
border-radius: 5;
background-color: qlineargradient(spread:pad, x1:0.507, y1:0.869318, x2:0.507, y2:0.0965909, stop:0.28 rgba(236, 236, 236, 255), stop:1 rgba(252, 252, 252, 255));
}
QPushButton:pressed {
border-style: inset;
background-color: qlineargradient(spread:pad, x1:0.502, y1:0.664864, x2:0.502, y2:0.034, stop:0.358209 rgba(250, 250, 250, 255), stop:0.626866 rgba(235, 235, 235, 255));
}
QPushButton:hover {
border: 1px outset #999;
border-color: rgb(83, 83, 83);
border-radius: 4;
}</string>
</property>
<property name="text">
<string>Apply Computed Values</string>
</property>
</widget>
<widget class="QLabel" name="label_21">
<property name="geometry">
<rect>
<x>40</x>
<y>410</y>
<width>104</width>
<height>20</height>
</rect>
</property>
<property name="text">
<string>Step Size</string>
</property>
</widget>
<widget class="QSlider" name="stepSizeSlider">
<property name="geometry">
<rect>
<x>149</x>
<y>410</y>
<width>266</width>
<height>20</height>
</rect>
</property>
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
<property name="objrelation" stdset="0">
<stringlist>
<string>objname:RelayTuningSettings</string>
<string>fieldname:Amplitude</string>
<string>scale:0.01</string>
<string>haslimits:no</string>
</stringlist>
</property>
</widget>
</widget>
<resources/>
<connections/>
</ui>

16
ground/openpilotgcs/src/plugins/config/config.pro
View File

@ -36,7 +36,8 @@ HEADERS += configplugin.h \
cfg_vehicletypes/configfixedwingwidget.h \
cfg_vehicletypes/vehicleconfig.h \
configrevowidget.h \
config_global.h
config_global.h \
configautotunewidget.h
SOURCES += configplugin.cpp \
configgadgetconfiguration.cpp \
configgadgetwidget.cpp \
@ -67,7 +68,8 @@ SOURCES += configplugin.cpp \
cfg_vehicletypes/configfixedwingwidget.cpp \
cfg_vehicletypes/configccpmwidget.cpp \
outputchannelform.cpp \
cfg_vehicletypes/vehicleconfig.cpp
cfg_vehicletypes/vehicleconfig.cpp \
configautotunewidget.cpp
FORMS += airframe.ui \
cc_hw_settings.ui \
pro_hw_settings.ui \
@ -83,5 +85,13 @@ FORMS += airframe.ui \
outputchannelform.ui \
revosensors.ui \
txpid.ui \
pipxtreme.ui
pipxtreme.ui \
autotune.ui
RESOURCES += configgadget.qrc

136
ground/openpilotgcs/src/plugins/config/configautotunewidget.cpp Normal file
View File

@ -0,0 +1,136 @@
#include "configautotunewidget.h"
#include <QDebug>
#include <QStringList>
#include <QtGui/QWidget>
#include <QtGui/QTextEdit>
#include <QtGui/QVBoxLayout>
#include <QtGui/QPushButton>
#include <QDesktopServices>
#include <QUrl>
#include <QList>
#include "relaytuningsettings.h"
#include "relaytuning.h"
#include "stabilizationsettings.h"
ConfigAutotuneWidget::ConfigAutotuneWidget(QWidget *parent) :
ConfigTaskWidget(parent)
{
m_autotune = new Ui_AutotuneWidget();
m_autotune->setupUi(this);
// Connect automatic signals
autoLoadWidgets();
disableMouseWheelEvents();
// Whenever any value changes compute new potential stabilization settings
connect(m_autotune->rateTuning, SIGNAL(valueChanged(int)), this, SLOT(recomputeStabilization()));
connect(m_autotune->attitudeTuning, SIGNAL(valueChanged(int)), this, SLOT(recomputeStabilization()));
RelayTuning *relayTuning = RelayTuning::GetInstance(getObjectManager());
Q_ASSERT(relayTuning);
if(relayTuning)
connect(relayTuning, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(recomputeStabilization()));
// Connect the apply button for the stabilization settings
connect(m_autotune->useComputedValues, SIGNAL(pressed()), this, SLOT(saveStabilization()));
}
/**
* Apply the stabilization settings computed
*/
void ConfigAutotuneWidget::saveStabilization()
{
StabilizationSettings *stabilizationSettings = StabilizationSettings::GetInstance(getObjectManager());
Q_ASSERT(stabilizationSettings);
if(!stabilizationSettings)
return;
// Make sure to recompute in case the other stab settings changed since
// the last time
recomputeStabilization();
// Apply this data to the board
stabilizationSettings->setData(stabSettings);
stabilizationSettings->updated();
}
/**
* Called whenever the gain ratios or measured values
* are changed
*/
void ConfigAutotuneWidget::recomputeStabilization()
{
RelayTuningSettings *relayTuningSettings = RelayTuningSettings::GetInstance(getObjectManager());
Q_ASSERT(relayTuningSettings);
if (!relayTuningSettings)
return;
RelayTuning *relayTuning = RelayTuning::GetInstance(getObjectManager());
Q_ASSERT(relayTuning);
if(!relayTuning)
return;
StabilizationSettings *stabilizationSettings = StabilizationSettings::GetInstance(getObjectManager());
Q_ASSERT(stabilizationSettings);
if(!stabilizationSettings)
return;
RelayTuning::DataFields relayTuningData = relayTuning->getData();
RelayTuningSettings::DataFields tuningSettingsData = relayTuningSettings->getData();
stabSettings = stabilizationSettings->getData();
// Need to divide these by 100 because that is what the .ui file does
// to get the UAVO
const double gain_ratio_r = m_autotune->rateTuning->value() / 100.0;
const double zero_ratio_r = m_autotune->rateTuning->value() / 100.0;
const double gain_ratio_p = m_autotune->attitudeTuning->value() / 100.0;
const double zero_ratio_p = m_autotune->attitudeTuning->value() / 100.0;
// For now just run over roll and pitch
for (int i = 0; i < 2; i++) {
if (relayTuningData.Period[i] == 0 || relayTuningData.Gain[i] == 0)
continue;
double wu = 1000.0 * 2 * M_PI / relayTuningData.Period[i]; // ultimate freq = output osc freq (rad/s)
double wc = wu * gain_ratio_r; // target openloop crossover frequency (rad/s)
double zc = wc * zero_ratio_r; // controller zero location (rad/s)
double kpu = 4.0f / M_PI / relayTuningData.Gain[i]; // ultimate gain, i.e. the proportional gain for instablity
double kp = kpu * gain_ratio_r; // proportional gain
double 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
double wc2 = wc * gain_ratio_p; // crossover of the attitude loop
double kp2 = wc2; // kp of attitude
double 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::PITCHRATEPID_KP] = kp;
stabSettings.PitchRatePID[StabilizationSettings::PITCHRATEPID_KI] = ki;
stabSettings.PitchPI[StabilizationSettings::PITCHPI_KP] = kp2;
stabSettings.PitchPI[StabilizationSettings::PITCHPI_KI] = ki2;
break;
}
}
// Display these computed settings
m_autotune->rollRateKp->setText(QString().number(stabSettings.RollRatePID[StabilizationSettings::ROLLRATEPID_KP]));
m_autotune->rollRateKi->setText(QString().number(stabSettings.RollRatePID[StabilizationSettings::ROLLRATEPID_KI]));
m_autotune->rollAttitudeKp->setText(QString().number(stabSettings.RollPI[StabilizationSettings::ROLLPI_KP]));
m_autotune->rollAttitudeKi->setText(QString().number(stabSettings.RollPI[StabilizationSettings::ROLLPI_KI]));
m_autotune->pitchRateKp->setText(QString().number(stabSettings.PitchRatePID[StabilizationSettings::PITCHRATEPID_KP]));
m_autotune->pitchRateKi->setText(QString().number(stabSettings.PitchRatePID[StabilizationSettings::PITCHRATEPID_KI]));
m_autotune->pitchAttitudeKp->setText(QString().number(stabSettings.PitchPI[StabilizationSettings::PITCHPI_KP]));
m_autotune->pitchAttitudeKi->setText(QString().number(stabSettings.PitchPI[StabilizationSettings::PITCHPI_KI]));
}

60
ground/openpilotgcs/src/plugins/config/configautotunewidget.h Normal file
View File

@ -0,0 +1,60 @@
/**
******************************************************************************
*
* @file configautotunewidget.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
* @addtogroup GCSPlugins GCS Plugins
* @{
* @addtogroup ConfigPlugin Config Plugin
* @{
* @brief The Configuration Gadget used to adjust or recalculate autotuning
*****************************************************************************/
/*
* 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 CONFIGAUTOTUNE_H
#define CONFIGAUTOTUNE_H
#include "ui_autotune.h"
#include "../uavobjectwidgetutils/configtaskwidget.h"
#include "extensionsystem/pluginmanager.h"
#include "uavobjectmanager.h"
#include "uavobject.h"
#include "stabilizationsettings.h"
#include "relaytuningsettings.h"
#include "relaytuning.h"
#include <QtGui/QWidget>
#include <QTimer>
class ConfigAutotuneWidget : public ConfigTaskWidget
{
Q_OBJECT
public:
explicit ConfigAutotuneWidget(QWidget *parent = 0);
private:
Ui_AutotuneWidget *m_autotune;
StabilizationSettings::DataFields stabSettings;
signals:
public slots:
private slots:
void recomputeStabilization();
void saveStabilization();
};
#endif // CONFIGAUTOTUNE_H

6
ground/openpilotgcs/src/plugins/config/configgadgetwidget.cpp
View File

@ -32,6 +32,7 @@
#include "configinputwidget.h"
#include "configoutputwidget.h"
#include "configstabilizationwidget.h"
#include "configautotunewidget.h"
#include "configcamerastabilizationwidget.h"
#include "configtxpidwidget.h"
#include "config_pro_hw_widget.h"
@ -84,6 +85,9 @@ ConfigGadgetWidget::ConfigGadgetWidget(QWidget *parent) : QWidget(parent)
qwd = new ConfigStabilizationWidget(this);
ftw->insertTab(ConfigGadgetWidget::stabilization, qwd, QIcon(":/configgadget/images/gyroscope.png"), QString("Stabilization"));
qwd = new ConfigAutotuneWidget(this);
ftw->insertTab(ConfigGadgetWidget::autotune, qwd, QIcon(":/configgadget/images/gyroscope.png"), QString("Autotune"));
qwd = new ConfigCameraStabilizationWidget(this);
ftw->insertTab(ConfigGadgetWidget::camerastabilization, qwd, QIcon(":/configgadget/images/camera.png"), QString("Camera Stab"));
@ -218,8 +222,6 @@ void ConfigGadgetWidget::tabAboutToChange(int i,bool * proceed)
*/
void ConfigGadgetWidget::updatePipXStatus(UAVObject *)
{
Q_UNUSED(object);
// Restart the disconnection timer.
pipxTimeout->start(5000);
if (!pipxConnected)

2
ground/openpilotgcs/src/plugins/config/configgadgetwidget.h
View File

@ -48,7 +48,7 @@ class ConfigGadgetWidget: public QWidget
public:
ConfigGadgetWidget(QWidget *parent = 0);
~ConfigGadgetWidget();
enum widgetTabs {hardware=0, aircraft, input, output, sensors, stabilization, camerastabilization, txpid, pipxtreme};
enum widgetTabs {hardware=0, aircraft, input, output, sensors, stabilization, camerastabilization, txpid, pipxtreme, autotune};
public slots:
void onAutopilotConnect();

8
ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro
View File

@ -71,6 +71,10 @@ HEADERS += $$UAVOBJECT_SYNTHETICS/accessorydesired.h \
$$UAVOBJECT_SYNTHETICS/velocitydesired.h \
$$UAVOBJECT_SYNTHETICS/velocityactual.h \
$$UAVOBJECT_SYNTHETICS/vtolpathfollowersettings.h \
$$UAVOBJECT_SYNTHETICS/guidancesettings.h \
$$UAVOBJECT_SYNTHETICS/positiondesired.h \
$$UAVOBJECT_SYNTHETICS/relaytuning.h \
$$UAVOBJECT_SYNTHETICS/relaytuningsettings.h \
$$UAVOBJECT_SYNTHETICS/ratedesired.h \
$$UAVOBJECT_SYNTHETICS/firmwareiapobj.h \
$$UAVOBJECT_SYNTHETICS/i2cstats.h \
@ -145,6 +149,10 @@ SOURCES += $$UAVOBJECT_SYNTHETICS/accessorydesired.cpp \
$$UAVOBJECT_SYNTHETICS/velocitydesired.cpp \
$$UAVOBJECT_SYNTHETICS/velocityactual.cpp \
$$UAVOBJECT_SYNTHETICS/vtolpathfollowersettings.cpp \
$$UAVOBJECT_SYNTHETICS/guidancesettings.cpp \
$$UAVOBJECT_SYNTHETICS/positiondesired.cpp \
$$UAVOBJECT_SYNTHETICS/relaytuningsettings.cpp \
$$UAVOBJECT_SYNTHETICS/relaytuning.cpp \
$$UAVOBJECT_SYNTHETICS/ratedesired.cpp \
$$UAVOBJECT_SYNTHETICS/firmwareiapobj.cpp \
$$UAVOBJECT_SYNTHETICS/i2cstats.cpp \

2
shared/uavobjectdefinition/flightstatus.xml
View File

@ -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,PathPlanner,RTH"/>
<field name="FlightMode" units="" type="enum" elements="1" options="Manual,Stabilized1,Stabilized2,Stabilized3,Autotune,AltitudeHold,VelocityControl,PositionHold,PathPlanner,RTH"/>
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="false" updatemode="manual" period="0"/>

2
shared/uavobjectdefinition/hwsettings.xml
View File

@ -18,7 +18,7 @@
<field name="USB_HIDPort" units="function" type="enum" elements="1" options="USBTelemetry,RCTransmitter,Disabled" defaultvalue="USBTelemetry"/>
<field name="USB_VCPPort" units="function" type="enum" elements="1" options="USBTelemetry,ComBridge,Disabled" defaultvalue="Disabled"/>
<field name="OptionalModules" units="" type="enum" elementnames="CameraStab,GPS,ComUsbBridge,Fault,Altitude,Airspeed,TxPID,VtolPathFollower,FixedWingPathFollower,Battery,Overo,Radio" options="Disabled,Enabled" defaultvalue="Disabled"/>
<field name="OptionalModules" units="" type="enum" elementnames="CameraStab,GPS,ComUsbBridge,Fault,Altitude,Airspeed,TxPID,Autotune,VtolPathFollower,FixedWingPathFollower,Battery,Overo,Radio" options="Disabled,Enabled" defaultvalue="Disabled"/>
<field name="DSMxBind" units="" type="uint8" elements="1" defaultvalue="0"/>
<access gcs="readwrite" flight="readwrite"/>

8
shared/uavobjectdefinition/manualcontrolsettings.xml
View File

@ -18,13 +18,13 @@
<field name="Arming" units="" type="enum" elements="1" options="Always Disarmed,Always Armed,Roll Left,Roll Right,Pitch Forward,Pitch Aft,Yaw Left,Yaw Right" defaultvalue="Always Disarmed"/>
<!-- Note these options should be identical to those in StabilizationDesired.StabilizationMode -->
<field name="Stabilization1Settings" units="" type="enum" elementnames="Roll,Pitch,Yaw" options="None,Rate,Attitude,AxisLock,WeakLeveling,VirtualBar" defaultvalue="Attitude,Attitude,Rate"/>
<field name="Stabilization2Settings" units="" type="enum" elementnames="Roll,Pitch,Yaw" options="None,Rate,Attitude,AxisLock,WeakLeveling,VirtualBar" defaultvalue="Attitude,Attitude,Rate"/>
<field name="Stabilization3Settings" units="" type="enum" elementnames="Roll,Pitch,Yaw" options="None,Rate,Attitude,AxisLock,WeakLeveling,VirtualBar" defaultvalue="Attitude,Attitude,Rate"/>
<field name="Stabilization1Settings" units="" type="enum" elementnames="Roll,Pitch,Yaw" options="None,Rate,Attitude,AxisLock,WeakLeveling,VirtualBar,RelayRate,RelayAttitude" defaultvalue="Attitude,Attitude,Rate"/>
<field name="Stabilization2Settings" units="" type="enum" elementnames="Roll,Pitch,Yaw" options="None,Rate,Attitude,AxisLock,WeakLeveling,VirtualBar,RelayRate,RelayAttitude" defaultvalue="Attitude,Attitude,Rate"/>
<field name="Stabilization3Settings" units="" type="enum" elementnames="Roll,Pitch,Yaw" options="None,Rate,Attitude,AxisLock,WeakLeveling,VirtualBar,RelayRate,RelayAttitude" defaultvalue="Attitude,Attitude,Rate"/>
<!-- 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,PathPlanner,RTH,Land" defaultvalue="Manual,Stabilized1,Stabilized2,Stabilized3,Stabilized1,Stabilized2" limits="%0401NE:AltitudeHold:VelocityControl:PositionHold;%0402NE:AltitudeHold:VelocityControl:PositionHold,%0401NE:AltitudeHold:VelocityControl:PositionHold;%0402NE:AltitudeHold:VelocityControl:PositionHold,%0401NE:AltitudeHold:VelocityControl:PositionHold;%0402NE:AltitudeHold:VelocityControl:PositionHold,%0401NE:AltitudeHold:VelocityControl:PositionHold;%0402NE:AltitudeHold:VelocityControl:PositionHold,%0401NE:AltitudeHold:VelocityControl:PositionHold;%0402NE:AltitudeHold:VelocityControl:PositionHold,%0401NE:AltitudeHold:VelocityControl:PositionHold;%0402NE:AltitudeHold:VelocityControl:PositionHold"/>
<field name="FlightModePosition" units="" type="enum" elements="6" options="Manual,Stabilized1,Stabilized2,Stabilized3,Autotune,AltitudeHold,VelocityControl,PositionHold,PathPlanner,RTH,Land" defaultvalue="Manual,Stabilized1,Stabilized2,Stabilized3,Stabilized1,Stabilized2" limits="%0401NE:AltitudeHold:VelocityControl:PositionHold;%0402NE:AltitudeHold:VelocityControl:PositionHold,%0401NE:AltitudeHold:VelocityControl:PositionHold;%0402NE:AltitudeHold:VelocityControl:PositionHold,%0401NE:AltitudeHold:VelocityControl:PositionHold;%0402NE:AltitudeHold:VelocityControl:PositionHold,%0401NE:AltitudeHold:VelocityControl:PositionHold;%0402NE:AltitudeHold:VelocityControl:PositionHold,%0401NE:AltitudeHold:VelocityControl:PositionHold;%0402NE:AltitudeHold:VelocityControl:PositionHold,%0401NE:AltitudeHold:VelocityControl:PositionHold;%0402NE:AltitudeHold:VelocityControl:PositionHold"/>
<field name="ArmedTimeout" units="ms" type="uint16" elements="1" defaultvalue="30000"/>
<field name="FailsafeBehavior" units="" type="enum" elements="1" options="None,RTH" defaultvalue="None"/>

11
shared/uavobjectdefinition/relaytuning.xml Normal file
View File

@ -0,0 +1,11 @@
<xml>
<object name="RelayTuning" singleinstance="true" settings="false">
<description>The input to the relay tuning.</description>
<field name="Period" units="ms" type="float" elementnames="Roll,Pitch,Yaw"/>
<field name="Gain" units="(deg/s)/output" type="float" elementnames="Roll,Pitch,Yaw"/>
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="false" updatemode="manual" period="0"/>
<telemetryflight acked="false" updatemode="periodic" period="1000"/>
<logging updatemode="manual" period="0"/>
</object>
</xml>

15
shared/uavobjectdefinition/relaytuningsettings.xml Normal file
View File

@ -0,0 +1,15 @@
<xml>
<object name="RelayTuningSettings" singleinstance="true" settings="true">
<description>Setting to run a relay tuning algorithm</description>
<field name="RateGain" units="" type="float" elements="1" defaultvalue="0.3333"/>
<field name="AttitudeGain" units="" type="float" elements="1" defaultvalue="0.2"/>
<field name="Amplitude" units="" type="float" elements="1" defaultvalue="0.25"/>
<field name="HysteresisThresh" units="deg/s" type="uint8" elements="1" defaultvalue="5"/>
<field name="Mode" units="" type="enum" elements="1" options="Rate,Attitude" defaultvalue="Attitude"/>
<field name="Behavior" units="" type="enum" elements="1" options="Measure,Compute,Save" defaultvalue="Compute"/>
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="true" updatemode="onchange" period="0"/>
<telemetryflight acked="true" updatemode="onchange" period="0"/>
<logging updatemode="manual" period="0"/>
</object>
</xml>

2
shared/uavobjectdefinition/stabilizationdesired.xml
View File

@ -6,7 +6,7 @@
<field name="Yaw" units="degrees" type="float" elements="1"/>
<field name="Throttle" units="%" type="float" elements="1"/>
<!-- These values should match those in ManualControlCommand.Stabilization{1,2,3}Settings -->
<field name="StabilizationMode" units="" type="enum" elementnames="Roll,Pitch,Yaw" options="None,Rate,Attitude,AxisLock,WeakLeveling,VirtualBar"/>
<field name="StabilizationMode" units="" type="enum" elementnames="Roll,Pitch,Yaw" options="None,Rate,Attitude,AxisLock,WeakLeveling,VirtualBar,RelayRate,RelayAttitude"/>
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="false" updatemode="manual" period="0"/>
<telemetryflight acked="false" updatemode="periodic" period="1000"/>

6
shared/uavobjectdefinition/taskinfo.xml
View File

@ -1,9 +1,9 @@
<xml>
<object name="TaskInfo" singleinstance="true" settings="false">
<description>Task information</description>
<field name="StackRemaining" units="bytes" type="uint16" elementnames="System,Actuator,Attitude,Sensors,TelemetryTx,TelemetryTxPri,TelemetryRx,GPS,ManualControl,Altitude,Airspeed,Stabilization,AltitudeHold,PathPlanner,PathFollower,FlightPlan,Com2UsbBridge,Usb2ComBridge,OveroSync,ModemRx,ModemTx,ModemStat,EventDispatcher"/>
<field name="Running" units="bool" type="enum" options="False,True" elementnames="System,Actuator,Attitude,Sensors,TelemetryTx,TelemetryTxPri,TelemetryRx,GPS,ManualControl,Altitude,Airspeed,Stabilization,AltitudeHold,PathPlanner,PathFollower,FlightPlan,Com2UsbBridge,Usb2ComBridge,OveroSync,ModemRx,ModemTx,ModemStat,EventDispatcher"/>
<field name="RunningTime" units="%" type="uint8" elementnames="System,Actuator,Attitude,Sensors,TelemetryTx,TelemetryTxPri,TelemetryRx,GPS,ManualControl,Altitude,Airspeed,Stabilization,AltitudeHold,PathPlanner,PathFollower,FlightPlan,Com2UsbBridge,Usb2ComBridge,OveroSync,ModemRx,ModemTx,ModemStat,EventDispatcher"/>
<field name="StackRemaining" units="bytes" type="uint16" elementnames="System,Actuator,Attitude,Sensors,TelemetryTx,TelemetryTxPri,TelemetryRx,GPS,ManualControl,Altitude,Airspeed,Stabilization,AltitudeHold,PathPlanner,PathFollower,FlightPlan,Com2UsbBridge,Usb2ComBridge,OveroSync,ModemRx,ModemTx,ModemStat,EventDispatcher,Autotune"/>
<field name="Running" units="bool" type="enum" options="False,True" elementnames="System,Actuator,Attitude,Sensors,TelemetryTx,TelemetryTxPri,TelemetryRx,GPS,ManualControl,Altitude,Airspeed,Stabilization,AltitudeHold,PathPlanner,PathFollower,FlightPlan,Com2UsbBridge,Usb2ComBridge,OveroSync,ModemRx,ModemTx,ModemStat,EventDispatcher,Autotune"/>
<field name="RunningTime" units="%" type="uint8" elementnames="System,Actuator,Attitude,Sensors,TelemetryTx,TelemetryTxPri,TelemetryRx,GPS,ManualControl,Altitude,Airspeed,Stabilization,AltitudeHold,PathPlanner,PathFollower,FlightPlan,Com2UsbBridge,Usb2ComBridge,OveroSync,ModemRx,ModemTx,ModemStat,EventDispatcher,Autotune"/>
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="true" updatemode="onchange" period="0"/>
<telemetryflight acked="true" updatemode="periodic" period="10000"/>