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

CameraStab: new camera stabilization mode and low-pass stick input filtering

Browse Source 
This patch is based on work of Crubier (LPF) and Cossacs (AxisLock mode).
I've just reworked it a bit by adding a dynamic memory allocation for
static module data.
This commit is contained in:
Oleg Semyonov 2011-12-16 22:27:13 +02:00
parent 21714911e8
commit 2eb5130a7c
2 changed files with 62 additions and 29 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

87
flight/Modules/CameraStab/camerastab.c
View File

@ -61,10 +61,17 @@
// Private types
// Private variables
static struct CameraStab_data {
UAVObjEvent ev;
portTickType lastSysTime;
float dT;
float inputs[CAMERASTABSETTINGS_INPUTS_NUMELEM];
float inputs_filtered[CAMERASTABSETTINGS_INPUTS_NUMELEM];
} *csd;
// Private functions
static void attitudeUpdated(UAVObjEvent* ev);
static float bound(float val);
static float bound(float val, float limit);
/**
* Initialise the module, called on startup
@ -72,8 +79,6 @@ static float bound(float val);
*/
int32_t CameraStabInitialize(void)
{
static UAVObjEvent ev;
bool cameraStabEnabled;
uint8_t optionalModules[HWSETTINGS_OPTIONALMODULES_NUMELEM];
@ -87,16 +92,26 @@ int32_t CameraStabInitialize(void)
if (cameraStabEnabled) {
// allocate and initialize the static data storage only if module is enabled
csd = (struct CameraStab_data *) pvPortMalloc(sizeof(struct CameraStab_data));
if (!csd)
return -1;
// make sure that all inputs[] and inputs_filtered[] are zeroed
memset(csd, 0, sizeof(struct CameraStab_data));
csd->lastSysTime = xTaskGetTickCount();
csd->dT = 1;
AttitudeActualInitialize();
ev.obj = AttitudeActualHandle();
ev.instId = 0;
ev.event = 0;
csd->ev.obj = AttitudeActualHandle();
csd->ev.instId = 0;
csd->ev.event = 0;
CameraStabSettingsInitialize();
CameraDesiredInitialize();
EventPeriodicCallbackCreate(&ev, attitudeUpdated, SAMPLE_PERIOD_MS / portTICK_RATE_MS);
EventPeriodicCallbackCreate(&(csd->ev), attitudeUpdated, SAMPLE_PERIOD_MS / portTICK_RATE_MS);
return 0;
}
@ -117,47 +132,61 @@ static void attitudeUpdated(UAVObjEvent* ev)
if (ev->obj != AttitudeActualHandle())
return;
float attitude;
float output;
AccessoryDesiredData accessory;
CameraStabSettingsData cameraStab;
CameraStabSettingsGet(&cameraStab);
// Read any input channels
float inputs[3] = {0,0,0};
if(cameraStab.Inputs[CAMERASTABSETTINGS_INPUTS_ROLL] != CAMERASTABSETTINGS_INPUTS_NONE) {
if(AccessoryDesiredInstGet(cameraStab.Inputs[CAMERASTABSETTINGS_INPUTS_ROLL] - CAMERASTABSETTINGS_INPUTS_ACCESSORY0, &accessory) == 0)
inputs[0] = accessory.AccessoryVal * cameraStab.InputRange[CAMERASTABSETTINGS_INPUTRANGE_ROLL];
}
if(cameraStab.Inputs[CAMERASTABSETTINGS_INPUTS_PITCH] != CAMERASTABSETTINGS_INPUTS_NONE) {
if(AccessoryDesiredInstGet(cameraStab.Inputs[CAMERASTABSETTINGS_INPUTS_PITCH] - CAMERASTABSETTINGS_INPUTS_ACCESSORY0, &accessory) == 0)
inputs[1] = accessory.AccessoryVal * cameraStab.InputRange[CAMERASTABSETTINGS_INPUTRANGE_PITCH];
}
if(cameraStab.Inputs[CAMERASTABSETTINGS_INPUTS_YAW] != CAMERASTABSETTINGS_INPUTS_NONE) {
if(AccessoryDesiredInstGet(cameraStab.Inputs[CAMERASTABSETTINGS_INPUTS_YAW] - CAMERASTABSETTINGS_INPUTS_ACCESSORY0, &accessory) == 0)
inputs[2] = accessory.AccessoryVal * cameraStab.InputRange[CAMERASTABSETTINGS_INPUTRANGE_YAW];
// Check how long since last update, time delta between calls in ms
portTickType thisSysTime = xTaskGetTickCount();
if (thisSysTime > csd->lastSysTime) // reuse dt in case of wraparound
csd->dT = (thisSysTime - csd->lastSysTime) / portTICK_RATE_MS;
csd->lastSysTime = thisSysTime;
// Read any input channels and apply LPF
for (uint8_t i = 0; i < CAMERASTABSETTINGS_INPUTS_NUMELEM; i++) {
if (cameraStab.Inputs[i] != CAMERASTABSETTINGS_INPUTS_NONE) {
if (AccessoryDesiredInstGet(cameraStab.Inputs[i] - CAMERASTABSETTINGS_INPUTS_ACCESSORY0, &accessory) == 0) {
float input_rate;
switch (cameraStab.StabilizationMode[i]) {
case CAMERASTABSETTINGS_STABILIZATIONMODE_ATTITUDE:
csd->inputs[i] = accessory.AccessoryVal * cameraStab.InputRange[i];
break;
case CAMERASTABSETTINGS_STABILIZATIONMODE_AXISLOCK:
input_rate = accessory.AccessoryVal * cameraStab.InputRate[i];
if (abs(input_rate) > cameraStab.MaxAxisLockRate)
csd->inputs[i] = bound(csd->inputs[i] + input_rate * csd->dT / 1000.0f, cameraStab.InputRange[i]);
break;
default:
PIOS_Assert(0);
}
csd->inputs_filtered[i] = (cameraStab.ResponseTime[i] / (cameraStab.ResponseTime[i] + csd->dT)) * csd->inputs_filtered[i]
+ (csd->dT / (cameraStab.ResponseTime[i] + csd->dT)) * csd->inputs[i];
}
}
}
// Set output channels
float attitude;
float output;
AttitudeActualRollGet(&attitude);
output = bound((attitude + inputs[0]) / cameraStab.OutputRange[CAMERASTABSETTINGS_OUTPUTRANGE_ROLL]);
output = bound((attitude + csd->inputs_filtered[CAMERASTABSETTINGS_INPUTS_ROLL]) / cameraStab.OutputRange[CAMERASTABSETTINGS_OUTPUTRANGE_ROLL], 1.0f);
CameraDesiredRollSet(&output);
AttitudeActualPitchGet(&attitude);
output = bound((attitude + inputs[1]) / cameraStab.OutputRange[CAMERASTABSETTINGS_OUTPUTRANGE_PITCH]);
output = bound((attitude + csd->inputs_filtered[CAMERASTABSETTINGS_INPUTS_PITCH]) / cameraStab.OutputRange[CAMERASTABSETTINGS_OUTPUTRANGE_PITCH], 1.0f);
CameraDesiredPitchSet(&output);
AttitudeActualYawGet(&attitude);
output = bound((attitude + inputs[2]) / cameraStab.OutputRange[CAMERASTABSETTINGS_OUTPUTRANGE_YAW]);
output = bound((attitude + csd->inputs_filtered[CAMERASTABSETTINGS_INPUTS_YAW]) / cameraStab.OutputRange[CAMERASTABSETTINGS_OUTPUTRANGE_YAW], 1.0f);
CameraDesiredYawSet(&output);
}
float bound(float val)
float bound(float val, float limit)
{
return (val > 1) ? 1 :
(val < -1) ? -1 :
return (val > limit) ? limit :
(val < -limit) ? -limit :
val;
}
/**

4
shared/uavobjectdefinition/camerastabsettings.xml
View File

@ -3,6 +3,10 @@
<description>Settings for the @ref CameraStab mmodule</description>
<field name="Inputs" units="channel" type="enum" elementnames="Roll,Pitch,Yaw" options="Accessory0,Accessory1,Accessory2,Accessory3,Accessory4,Accessory5,,None" defaultvalue="None"/>
<field name="InputRange" units="deg" type="uint8" elementnames="Roll,Pitch,Yaw" defaultvalue="20"/>
<field name="InputRate" units="deg/s" type="uint8" elementnames="Roll,Pitch,Yaw" defaultvalue="50"/>
<field name="ResponseTime" units="ms" type="float" elementnames="Roll,Pitch,Yaw" defaultvalue="150"/>
<field name="StabilizationMode" units="" type="enum" elementnames="Roll,Pitch,Yaw" options="Attitude,AxisLock" defaultvalue="Attitude"/>
<field name="MaxAxisLockRate" units="deg/s" type="float" elements="1" defaultvalue="1"/>
<field name="OutputRange" units="deg" type="uint8" elementnames="Roll,Pitch,Yaw" defaultvalue="20"/>
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="true" updatemode="onchange" period="0"/>