mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2025-03-21 13:28:58 +01:00
Factor the virtual flybar code out of the stabilization module
This commit is contained in:
James Cotton
parent
aae0e562c6
commit
2723ff4be3
@ -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
42
flight/Modules/Stabilization/inc/virtualflybar.h
Normal 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 */
|
||||
@ -47,6 +47,7 @@
|
||||
|
||||
// Includes for various stabilization algorithms
|
||||
#include "relay_tuning.h"
|
||||
#include "virtualflybar.h"
|
||||
|
||||
// Private constants
|
||||
#define MAX_QUEUE_SIZE 1
|
||||
@ -62,9 +63,6 @@
|
||||
|
||||
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 {ROLL,PITCH,YAW,MAX_AXES};
|
||||
|
||||
|
||||
// Private types
|
||||
typedef struct {
|
||||
float p;
|
||||
@ -80,7 +78,6 @@ 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;
|
||||
@ -88,11 +85,8 @@ 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;
|
||||
|
||||
// Private functions
|
||||
static void stabilizationTask(void* parameters);
|
||||
@ -224,6 +218,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);
|
||||
@ -278,32 +273,14 @@ static void stabilizationTask(void* parameters)
|
||||
break;
|
||||
|
||||
case STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR:
|
||||
{
|
||||
if(reinit)
|
||||
vbar_integral[i] = 0;
|
||||
|
||||
// Store for debugging output
|
||||
rateDesiredAxis[i] = attitudeDesiredAxis[i];
|
||||
|
||||
// Track the angle of the virtual flybar which includes a slow decay
|
||||
vbar_integral[i] = vbar_integral[i] * vbar_decay + gyro_filtered[i] * dT;
|
||||
vbar_integral[i] = bound(vbar_integral[i], settings.VbarMaxAngle);
|
||||
|
||||
// Command signal can indicate how much to disregard the gyro feedback (fast flips)
|
||||
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;
|
||||
}
|
||||
|
||||
// Command signal is composed of stick input added to the gyro and virtual flybar
|
||||
actuatorDesiredAxis[i] = 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(actuatorDesiredAxis[i],1.0f);
|
||||
// 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)
|
||||
@ -374,19 +351,8 @@ static void stabilizationTask(void* parameters)
|
||||
}
|
||||
}
|
||||
|
||||
// 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);
|
||||
|
||||
#if defined(DIAGNOSTICS)
|
||||
RateDesiredSet(&rateDesired);
|
||||
@ -544,8 +510,6 @@ static void SettingsUpdatedCb(UAVObjEvent * ev)
|
||||
|
||||
// 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;
|
||||
}
|
||||
|
||||
|
||||
|
||||
119
flight/Modules/Stabilization/virtualflybar.c
Normal file
119
flight/Modules/Stabilization/virtualflybar.c
Normal 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;
|
||||
}
|
||||
Loading…
x
Reference in New Issue
Block a user