rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-01-30 15:52:12 +01:00
Code Issues Releases Activity

OP-1848 altvari0

Browse Source 
1. build fixes
2. Updated ui to use updated uavo names in altitude settings that align to the naming in vtol settings
3. Moved call the thrust altvario to pre application of PIDs for 3d axes to allow for later coupling.
This commit is contained in:
abeck70 2015-04-23 21:13:00 +10:00
parent d469a754bf
commit 0d77cd95d9
6 changed files with 137 additions and 124 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

215
flight/modules/Stabilization/outerloop.c
View File

@ -103,7 +103,25 @@ static void stabilizationOuterloopTask()
float *stabilizationDesiredAxis = &stabilizationDesired.Roll;
float *rateDesiredAxis = &rateDesired.Roll;
int t;
float dT = PIOS_DELTATIME_GetAverageSeconds(&timeval);
float dT = PIOS_DELTATIME_GetAverageSeconds(&timeval);
bool reinit = (StabilizationStatusOuterLoopToArray(enabled)[STABILIZATIONSTATUS_OUTERLOOP_THRUST] != previous_mode[STABILIZATIONSTATUS_OUTERLOOP_THRUST]);
previous_mode[STABILIZATIONSTATUS_OUTERLOOP_THRUST] = StabilizationStatusOuterLoopToArray(enabled)[STABILIZATIONSTATUS_OUTERLOOP_THRUST];
switch (StabilizationStatusOuterLoopToArray(enabled)[STABILIZATIONSTATUS_OUTERLOOP_THRUST]) {
#ifdef REVOLUTION
case STABILIZATIONSTATUS_OUTERLOOP_ALTITUDE:
rateDesiredAxis[STABILIZATIONSTATUS_OUTERLOOP_THRUST] = stabilizationAltitudeHold(stabilizationDesiredAxis[STABILIZATIONSTATUS_OUTERLOOP_THRUST], ALTITUDEHOLD, reinit);
break;
case STABILIZATIONSTATUS_OUTERLOOP_ALTITUDEVARIO:
rateDesiredAxis[STABILIZATIONSTATUS_OUTERLOOP_THRUST] = stabilizationAltitudeHold(stabilizationDesiredAxis[STABILIZATIONSTATUS_OUTERLOOP_THRUST], ALTITUDEVARIO, reinit);
break;
#endif /* REVOLUTION */
case STABILIZATIONSTATUS_OUTERLOOP_DIRECT:
default:
rateDesiredAxis[STABILIZATIONSTATUS_OUTERLOOP_THRUST] = stabilizationDesiredAxis[STABILIZATIONSTATUS_OUTERLOOP_THRUST];
break;
}
float local_error[3];
{
@ -148,117 +166,102 @@ static void stabilizationOuterloopTask()
}
#endif /* if defined(PIOS_QUATERNION_STABILIZATION) */
}
for (t = 0; t < AXES; t++) {
bool reinit = (StabilizationStatusOuterLoopToArray(enabled)[t] != previous_mode[t]);
for (t = STABILIZATIONSTATUS_OUTERLOOP_ROLL; t < STABILIZATIONSTATUS_OUTERLOOP_THRUST; t++) {
reinit = (StabilizationStatusOuterLoopToArray(enabled)[t] != previous_mode[t]);
previous_mode[t] = StabilizationStatusOuterLoopToArray(enabled)[t];
if (t < STABILIZATIONSTATUS_OUTERLOOP_THRUST) {
if (reinit) {
stabSettings.outerPids[t].iAccumulator = 0;
}
switch (StabilizationStatusOuterLoopToArray(enabled)[t]) {
case STABILIZATIONSTATUS_OUTERLOOP_ATTITUDE:
rateDesiredAxis[t] = pid_apply(&stabSettings.outerPids[t], local_error[t], dT);
break;
case STABILIZATIONSTATUS_OUTERLOOP_RATTITUDE:
{
float stickinput[3];
stickinput[0] = boundf(stabilizationDesiredAxis[0] / stabSettings.stabBank.RollMax, -1.0f, 1.0f);
stickinput[1] = boundf(stabilizationDesiredAxis[1] / stabSettings.stabBank.PitchMax, -1.0f, 1.0f);
stickinput[2] = boundf(stabilizationDesiredAxis[2] / stabSettings.stabBank.YawMax, -1.0f, 1.0f);
float rateDesiredAxisRate = stickinput[t] * StabilizationBankManualRateToArray(stabSettings.stabBank.ManualRate)[t];
// limit corrective rate to maximum rates to not give it overly large impact over manual rate when joined together
rateDesiredAxis[t] = boundf(pid_apply(&stabSettings.outerPids[t], local_error[t], dT),
-StabilizationBankManualRateToArray(stabSettings.stabBank.ManualRate)[t],
StabilizationBankManualRateToArray(stabSettings.stabBank.ManualRate)[t]
);
// Compute the weighted average rate desired
// Using max() rather than sqrt() for cpu speed;
// - this makes the stick region into a square;
// - this is a feature!
// - hold a roll angle and add just pitch without the stick sensitivity changing
float magnitude = fabsf(stickinput[t]);
if (t < 2) {
magnitude = fmaxf(fabsf(stickinput[0]), fabsf(stickinput[1]));
}
// modify magnitude to move the Att to Rate transition to the place
// specified by the user
// we are looking for where the stick angle == transition angle
// and the Att rate equals the Rate rate
// that's where Rate x (1-StickAngle) [Attitude pulling down max X Ratt proportion]
// == Rate x StickAngle [Rate pulling up according to stick angle]
// * StickAngle [X Ratt proportion]
// so 1-x == x*x or x*x+x-1=0 where xE(0,1)
// (-1+-sqrt(1+4))/2 = (-1+sqrt(5))/2
// and quadratic formula says that is 0.618033989f
// I tested 14.01 and came up with .61 without even remembering this number
// I thought that moving the P,I, and maxangle terms around would change this value
// and that I would have to take these into account, but varying
// all P's and I's by factors of 1/2 to 2 didn't change it noticeably
// and varying maxangle from 4 to 120 didn't either.
// so for now I'm not taking these into account
// while working with this, it occurred to me that Attitude mode,
// set up with maxangle=190 would be similar to Ratt, and it is.
#define STICK_VALUE_AT_MODE_TRANSITION 0.618033989f
// the following assumes the transition would otherwise be at 0.618033989f
// and THAT assumes that Att ramps up to max roll rate
// when a small number of degrees off of where it should be
// if below the transition angle (still in attitude mode)
// '<=' instead of '<' keeps rattitude_mode_transition_stick_position==1.0 from causing DZ
if (magnitude <= stabSettings.rattitude_mode_transition_stick_position) {
magnitude *= STICK_VALUE_AT_MODE_TRANSITION / stabSettings.rattitude_mode_transition_stick_position;
} else {
magnitude = (magnitude - stabSettings.rattitude_mode_transition_stick_position)
* (1.0f - STICK_VALUE_AT_MODE_TRANSITION)
/ (1.0f - stabSettings.rattitude_mode_transition_stick_position)
+ STICK_VALUE_AT_MODE_TRANSITION;
}
rateDesiredAxis[t] = (1.0f - magnitude) * rateDesiredAxis[t] + magnitude * rateDesiredAxisRate;
}
if (reinit) {
stabSettings.outerPids[t].iAccumulator = 0;
}
switch (StabilizationStatusOuterLoopToArray(enabled)[t]) {
case STABILIZATIONSTATUS_OUTERLOOP_ATTITUDE:
rateDesiredAxis[t] = pid_apply(&stabSettings.outerPids[t], local_error[t], dT);
break;
case STABILIZATIONSTATUS_OUTERLOOP_WEAKLEVELING:
// FIXME: local_error[] is rate - attitude for Weak Leveling
// The only ramifications are:
// Weak Leveling Kp is off by a factor of 3 to 12 and may need a different default in GCS
// Changing Rate mode max rate currently requires a change to Kp
// That would be changed to Attitude mode max angle affecting Kp
// Also does not take dT into account
{
float stickinput[3];
stickinput[0] = boundf(stabilizationDesiredAxis[0] / stabSettings.stabBank.RollMax, -1.0f, 1.0f);
stickinput[1] = boundf(stabilizationDesiredAxis[1] / stabSettings.stabBank.PitchMax, -1.0f, 1.0f);
stickinput[2] = boundf(stabilizationDesiredAxis[2] / stabSettings.stabBank.YawMax, -1.0f, 1.0f);
float rate_input = stickinput[t] * StabilizationBankManualRateToArray(stabSettings.stabBank.ManualRate)[t];
float weak_leveling = local_error[t] * stabSettings.settings.WeakLevelingKp;
weak_leveling = boundf(weak_leveling, -stabSettings.settings.MaxWeakLevelingRate, stabSettings.settings.MaxWeakLevelingRate);
case STABILIZATIONSTATUS_OUTERLOOP_RATTITUDE:
{
float stickinput[3];
stickinput[0] = boundf(stabilizationDesiredAxis[0] / stabSettings.stabBank.RollMax, -1.0f, 1.0f);
stickinput[1] = boundf(stabilizationDesiredAxis[1] / stabSettings.stabBank.PitchMax, -1.0f, 1.0f);
stickinput[2] = boundf(stabilizationDesiredAxis[2] / stabSettings.stabBank.YawMax, -1.0f, 1.0f);
float rateDesiredAxisRate = stickinput[t] * StabilizationBankManualRateToArray(stabSettings.stabBank.ManualRate)[t];
// limit corrective rate to maximum rates to not give it overly large impact over manual rate when joined together
rateDesiredAxis[t] = boundf(pid_apply(&stabSettings.outerPids[t], local_error[t], dT),
-StabilizationBankManualRateToArray(stabSettings.stabBank.ManualRate)[t],
StabilizationBankManualRateToArray(stabSettings.stabBank.ManualRate)[t]
);
// Compute the weighted average rate desired
// Using max() rather than sqrt() for cpu speed;
// - this makes the stick region into a square;
// - this is a feature!
// - hold a roll angle and add just pitch without the stick sensitivity changing
float magnitude = fabsf(stickinput[t]);
if (t < 2) {
magnitude = fmaxf(fabsf(stickinput[0]), fabsf(stickinput[1]));
}
// Compute desired rate as input biased towards leveling
rateDesiredAxis[t] = rate_input + weak_leveling;
// modify magnitude to move the Att to Rate transition to the place
// specified by the user
// we are looking for where the stick angle == transition angle
// and the Att rate equals the Rate rate
// that's where Rate x (1-StickAngle) [Attitude pulling down max X Ratt proportion]
// == Rate x StickAngle [Rate pulling up according to stick angle]
// * StickAngle [X Ratt proportion]
// so 1-x == x*x or x*x+x-1=0 where xE(0,1)
// (-1+-sqrt(1+4))/2 = (-1+sqrt(5))/2
// and quadratic formula says that is 0.618033989f
// I tested 14.01 and came up with .61 without even remembering this number
// I thought that moving the P,I, and maxangle terms around would change this value
// and that I would have to take these into account, but varying
// all P's and I's by factors of 1/2 to 2 didn't change it noticeably
// and varying maxangle from 4 to 120 didn't either.
// so for now I'm not taking these into account
// while working with this, it occurred to me that Attitude mode,
// set up with maxangle=190 would be similar to Ratt, and it is.
#define STICK_VALUE_AT_MODE_TRANSITION 0.618033989f
// the following assumes the transition would otherwise be at 0.618033989f
// and THAT assumes that Att ramps up to max roll rate
// when a small number of degrees off of where it should be
// if below the transition angle (still in attitude mode)
// '<=' instead of '<' keeps rattitude_mode_transition_stick_position==1.0 from causing DZ
if (magnitude <= stabSettings.rattitude_mode_transition_stick_position) {
magnitude *= STICK_VALUE_AT_MODE_TRANSITION / stabSettings.rattitude_mode_transition_stick_position;
} else {
magnitude = (magnitude - stabSettings.rattitude_mode_transition_stick_position)
* (1.0f - STICK_VALUE_AT_MODE_TRANSITION)
/ (1.0f - stabSettings.rattitude_mode_transition_stick_position)
+ STICK_VALUE_AT_MODE_TRANSITION;
}
rateDesiredAxis[t] = (1.0f - magnitude) * rateDesiredAxis[t] + magnitude * rateDesiredAxisRate;
}
break;
case STABILIZATIONSTATUS_OUTERLOOP_WEAKLEVELING:
// FIXME: local_error[] is rate - attitude for Weak Leveling
// The only ramifications are:
// Weak Leveling Kp is off by a factor of 3 to 12 and may need a different default in GCS
// Changing Rate mode max rate currently requires a change to Kp
// That would be changed to Attitude mode max angle affecting Kp
// Also does not take dT into account
{
float stickinput[3];
stickinput[0] = boundf(stabilizationDesiredAxis[0] / stabSettings.stabBank.RollMax, -1.0f, 1.0f);
stickinput[1] = boundf(stabilizationDesiredAxis[1] / stabSettings.stabBank.PitchMax, -1.0f, 1.0f);
stickinput[2] = boundf(stabilizationDesiredAxis[2] / stabSettings.stabBank.YawMax, -1.0f, 1.0f);
float rate_input = stickinput[t] * StabilizationBankManualRateToArray(stabSettings.stabBank.ManualRate)[t];
float weak_leveling = local_error[t] * stabSettings.settings.WeakLevelingKp;
weak_leveling = boundf(weak_leveling, -stabSettings.settings.MaxWeakLevelingRate, stabSettings.settings.MaxWeakLevelingRate);
// Compute desired rate as input biased towards leveling
rateDesiredAxis[t] = rate_input + weak_leveling;
}
break;
case STABILIZATIONSTATUS_OUTERLOOP_DIRECT:
default:
rateDesiredAxis[t] = stabilizationDesiredAxis[t];
break;
case STABILIZATIONSTATUS_OUTERLOOP_DIRECT:
default:
rateDesiredAxis[t] = stabilizationDesiredAxis[t];
break;
}
} else {
switch (StabilizationStatusOuterLoopToArray(enabled)[t]) {
#ifdef REVOLUTION
case STABILIZATIONSTATUS_OUTERLOOP_ALTITUDE:
rateDesiredAxis[t] = stabilizationAltitudeHold(stabilizationDesiredAxis[t], ALTITUDEHOLD, reinit);
break;
case STABILIZATIONSTATUS_OUTERLOOP_ALTITUDEVARIO:
rateDesiredAxis[t] = stabilizationAltitudeHold(stabilizationDesiredAxis[t], ALTITUDEVARIO, reinit);
break;
#endif /* REVOLUTION */
case STABILIZATIONSTATUS_OUTERLOOP_DIRECT:
default:
rateDesiredAxis[t] = stabilizationDesiredAxis[t];
break;
}
}
}

6
flight/targets/boards/discoveryf4bare/firmware/Makefile
View File

@ -24,6 +24,9 @@ endif
include ../board-info.mk
include $(ROOT_DIR)/make/firmware-defs.mk
# REVO C++ support
USE_CXX = YES
# ARM DSP library
USE_DSP_LIB ?= NO
@ -70,7 +73,7 @@ ifndef TESTAPP
## Application Core
SRC += ../pios_usb_board_data.c
SRC += $(OPMODULEDIR)/System/systemmod.c
SRC += $(OPSYSTEM)/discoveryf4bare.c
CPPSRC += $(OPSYSTEM)/discoveryf4bare.cpp
SRC += $(OPSYSTEM)/pios_board.c
SRC += $(FLIGHTLIB)/alarms.c
SRC += $(OPUAVTALK)/uavtalk.c
@ -91,6 +94,7 @@ ifndef TESTAPP
SRC += $(FLIGHTLIB)/insgps13state.c
SRC += $(FLIGHTLIB)/auxmagsupport.c
SRC += $(FLIGHTLIB)/lednotification.c
CPPSRC += $(FLIGHTLIB)/mini_cpp.cpp
## UAVObjects
include ./UAVObjects.inc

3
flight/targets/boards/discoveryf4bare/firmware/discoveryf4bare.c → flight/targets/boards/discoveryf4bare/firmware/discoveryf4bare.cpp
View File

@ -31,7 +31,7 @@
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
extern "C" {
#include "inc/openpilot.h"
#include <uavobjectsinit.h>
@ -74,6 +74,7 @@ static void initTask(void *parameters);
/* Prototype of generated InitModules() function */
extern void InitModules(void);
}
/**
* OpenPilot Main function:

7
flight/targets/boards/revoproto/firmware/Makefile
View File

@ -24,6 +24,9 @@ endif
include ../board-info.mk
include $(ROOT_DIR)/make/firmware-defs.mk
# REVO C++ support
USE_CXX = YES
# ARM DSP library
USE_DSP_LIB ?= NO
@ -65,7 +68,7 @@ ifndef TESTAPP
## Application Core
SRC += ../pios_usb_board_data.c
SRC += $(OPMODULEDIR)/System/systemmod.c
SRC += $(OPSYSTEM)/revolution.c
CPPSRC += $(OPSYSTEM)/revolution.cpp
SRC += $(OPSYSTEM)/pios_board.c
SRC += $(FLIGHTLIB)/alarms.c
SRC += $(OPUAVTALK)/uavtalk.c
@ -84,6 +87,8 @@ ifndef TESTAPP
SRC += $(FLIGHTLIB)/WorldMagModel.c
SRC += $(FLIGHTLIB)/insgps13state.c
SRC += $(FLIGHTLIB)/auxmagsupport.c
CPPSRC += $(FLIGHTLIB)/mini_cpp.cpp
## UAVObjects
include ./UAVObjects.inc

2
flight/targets/boards/revoproto/firmware/revolution.c → flight/targets/boards/revoproto/firmware/revolution.cpp
View File

@ -31,6 +31,7 @@
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
extern "C" {
#include "inc/openpilot.h"
#include <uavobjectsinit.h>
@ -73,6 +74,7 @@ static void initTask(void *parameters);
/* Prototype of generated InitModules() function */
extern void InitModules(void);
}
/**
* OpenPilot Main function:

28
ground/openpilotgcs/src/plugins/config/stabilization.ui
View File

@ -25209,8 +25209,7 @@ border-radius: 5;</string>
<property name="objrelation" stdset="0">
<stringlist>
<string>objname:AltitudeHoldSettings</string>
<string>fieldname:AltitudePI</string>
<string>element:Kp</string>
<string>fieldname:VerticalPosP</string>
<string>scale:0.01</string>
<string>haslimits:yes</string>
<string>buttongroup:98,10</string>
@ -25294,7 +25293,7 @@ border-radius: 5;</string>
</widget>
</item>
<item row="3" column="1">
<widget class="QSlider" name="AltKdSlider">
<widget class="QSlider" name="VelKiSlider">
<property name="toolTip">
<string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;How fast the vehicle should adjust its neutral throttle estimation. Altitude assumes that when engaged the throttle is in the range required to hover. If the throttle is a lot higher or lower, it needs to adjust this &amp;quot;throttle trim&amp;quot; Higher values make it do this adjustment faster, but this could lead to ugly oscillations. Leave at default unless you know what you are doing.&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
</property>
@ -25313,7 +25312,7 @@ border-radius: 5;</string>
<property name="objrelation" stdset="0">
<stringlist>
<string>objname:AltitudeHoldSettings</string>
<string>fieldname:VelocityPI</string>
<string>fieldname:VerticalVelPID</string>
<string>element:Ki</string>
<string>scale:0.00001</string>
<string>haslimits:yes</string>
@ -25323,7 +25322,7 @@ border-radius: 5;</string>
</widget>
</item>
<item row="2" column="2">
<widget class="QSpinBox" name="AltKi">
<widget class="QSpinBox" name="VelKp">
<property name="sizePolicy">
<sizepolicy hsizetype="Preferred" vsizetype="Fixed">
<horstretch>0</horstretch>
@ -25370,7 +25369,7 @@ border-radius: 5;</string>
</widget>
</item>
<item row="3" column="2">
<widget class="QSpinBox" name="AltKd">
<widget class="QSpinBox" name="VelKi">
<property name="sizePolicy">
<sizepolicy hsizetype="Preferred" vsizetype="Fixed">
<horstretch>0</horstretch>
@ -25434,10 +25433,9 @@ border-radius: 5;</string>
<enum>QSlider::TicksBelow</enum>
</property>
<property name="objrelation" stdset="0">
<stringlist>
<stringlist notr="true">
<string>objname:AltitudeHoldSettings</string>
<string>fieldname:AltitudePI</string>
<string>element:Kp</string>
<string>fieldname:VerticalPosP</string>
<string>scale:0.01</string>
<string>haslimits:yes</string>
<string>buttongroup:98</string>
@ -25446,7 +25444,7 @@ border-radius: 5;</string>
</widget>
</item>
<item row="2" column="1">
<widget class="QSlider" name="AltKiSlider">
<widget class="QSlider" name="VelKpSlider">
<property name="toolTip">
<string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;How much the vehicle should throttle up or down to compensate or achieve a certain vertical speed. Higher values lead to more aggressive throttle changes and could lead to oscillations. This is the most likely candidate to change depending on the crafts engine thrust. Heavy craft with weak engines might require higher values.&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
</property>
@ -25465,7 +25463,7 @@ border-radius: 5;</string>
<property name="objrelation" stdset="0">
<stringlist>
<string>objname:AltitudeHoldSettings</string>
<string>fieldname:VelocityPI</string>
<string>fieldname:VerticalVelPID</string>
<string>element:Kp</string>
<string>scale:0.01</string>
<string>haslimits:yes</string>
@ -27173,10 +27171,10 @@ Useful if you have accidentally changed some settings.</string>
<tabstop>pushButton_7</tabstop>
<tabstop>AltKpSlider</tabstop>
<tabstop>AltKp</tabstop>
<tabstop>AltKiSlider</tabstop>
<tabstop>AltKi</tabstop>
<tabstop>AltKdSlider</tabstop>
<tabstop>AltKd</tabstop>
<tabstop>VelKpSlider</tabstop>
<tabstop>VelKp</tabstop>
<tabstop>VelKiSlider</tabstop>
<tabstop>VelKi</tabstop>
<tabstop>pushButton_8</tabstop>
<tabstop>AltThrExpSlider_2</tabstop>
<tabstop>AltThrExp_2</tabstop>