rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-27 16:54:15 +01:00
Code Issues Releases Activity

Merged in f5soh/librepilot/LP-205_Rate_trainer_wobble (pull request #148)

Browse Source 
LP-205 Rate trainer : add Attitude stabilization at max angle
This commit is contained in:
Lalanne Laurent 2016-01-24 17:54:34 +01:00
commit 27afc79767
1 changed files with 44 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -64,6 +64,10 @@ static AttitudeStateData attitude;
static uint8_t previous_mode[AXES] = { 255, 255, 255, 255 }; static uint8_t previous_mode[AXES] = { 255, 255, 255, 255 };
static PiOSDeltatimeConfig timeval; static PiOSDeltatimeConfig timeval;
static bool pitchMin = false;
static bool pitchMax = false;
static bool rollMin = false;
static bool rollMax = false;
// Private functions // Private functions
static void stabilizationOuterloopTask(); static void stabilizationOuterloopTask();
@ -281,31 +285,51 @@ static void stabilizationOuterloopTask()
rateDesiredAxis[t] = stabilizationDesiredAxis[t]; // default for all axes rateDesiredAxis[t] = stabilizationDesiredAxis[t]; // default for all axes
// now test limits for pitch and/or roll // now test limits for pitch and/or roll
if (t == 1) { // pitch if (t == 1) { // pitch
if (attitudeState.Pitch < -stabSettings.stabBank.PitchMax) { if ((attitudeState.Pitch < -stabSettings.stabBank.PitchMax) || pitchMin) {
// attitude exceeds pitch max. pitchMin = true;
// zero rate desired if also -ve // Attitude exceeds pitch min,
if (rateDesiredAxis[t] < 0.0f) { // Do Attitude stabilisation at min pitch angle while user still maintain negative pitch
rateDesiredAxis[t] = 0.0f; if (stabilizationDesiredAxis[t] < 0.0f) {
local_error[t] = -stabSettings.stabBank.PitchMax - attitudeState.Pitch;
rateDesiredAxis[t] = pid_apply(&stabSettings.outerPids[t], local_error[t], dT);
} else {
// Stop Attitude stabilization and return to Rate
pitchMin = false;
} }
} else if (attitudeState.Pitch > stabSettings.stabBank.PitchMax) { } else if ((attitudeState.Pitch > stabSettings.stabBank.PitchMax) || pitchMax) {
// attitude exceeds pitch max pitchMax = true;
// zero rate desired if also +ve // Attitude exceeds pitch max
if (rateDesiredAxis[t] > 0.0f) { // Do Attitude stabilisation at max pitch angle while user still maintain positive pitch
rateDesiredAxis[t] = 0.0f; if (stabilizationDesiredAxis[t] > 0.0f) {
local_error[t] = stabSettings.stabBank.PitchMax - attitudeState.Pitch;
rateDesiredAxis[t] = pid_apply(&stabSettings.outerPids[t], local_error[t], dT);
} else {
// Stop Attitude stabilization and return to Rate
pitchMax = false;
} }
} }
} else if (t == 0) { // roll } else if (t == 0) { // roll
if (attitudeState.Roll < -stabSettings.stabBank.RollMax) { if ((attitudeState.Roll < -stabSettings.stabBank.RollMax) || rollMin) {
// attitude exceeds roll max. rollMin = true;
// zero rate desired if also -ve // Attitude exceeds roll min,
if (rateDesiredAxis[t] < 0.0f) { // Do Attitude stabilisation at min roll angle while user still maintain negative roll
rateDesiredAxis[t] = 0.0f; if (stabilizationDesiredAxis[t] < 0.0f) {
local_error[t] = -stabSettings.stabBank.RollMax - attitudeState.Roll;
rateDesiredAxis[t] = pid_apply(&stabSettings.outerPids[t], local_error[t], dT);
} else {
// Stop Attitude stabilization and return to Rate
rollMin = false;
} }
} else if (attitudeState.Roll > stabSettings.stabBank.RollMax) { } else if ((attitudeState.Roll > stabSettings.stabBank.RollMax) || rollMax) {
// attitude exceeds roll max rollMax = true;
// zero rate desired if also +ve // Attitude exceeds roll max
if (rateDesiredAxis[t] > 0.0f) { // Do Attitude stabilisation at max roll angle while user still maintain positive roll
rateDesiredAxis[t] = 0.0f; if (stabilizationDesiredAxis[t] > 0.0f) {
local_error[t] = stabSettings.stabBank.RollMax - attitudeState.Roll;
rateDesiredAxis[t] = pid_apply(&stabSettings.outerPids[t], local_error[t], dT);
} else {
// Stop Attitude stabilization and return to Rate
rollMax = false;
} }
} }
} }