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

OP-1309 provide faster GyroState updates for more prompt response and less stabilization lag

Browse Source
This commit is contained in:
Corvus Corax 2014-05-01 17:43:23 +02:00
parent 936a52112a
commit 944f3f5d2f
1 changed files with 23 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
flight/modules/StateEstimation/stateestimation.c
View File

@ -155,6 +155,10 @@ static stateFilter cfmFilter;
static stateFilter ekf13iFilter; static stateFilter ekf13iFilter;
static stateFilter ekf13Filter; static stateFilter ekf13Filter;
// this is a hack to provide a computational shortcut for faster gyro state progression
static float gyroRaw[3];
static float gyroDelta[3];
// preconfigured filter chains selectable via revoSettings.FusionAlgorithm // preconfigured filter chains selectable via revoSettings.FusionAlgorithm
static const filterPipeline *cfQueue = &(filterPipeline) { static const filterPipeline *cfQueue = &(filterPipeline) {
.filter = &magFilter, .filter = &magFilter,
@ -415,6 +419,11 @@ static void StateEstimationCb(void)
// fetch sensors, check values, and load into state struct // fetch sensors, check values, and load into state struct
FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_3_DIMENSIONS(GyroSensor, gyro, x, y, z); FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_3_DIMENSIONS(GyroSensor, gyro, x, y, z);
if (IS_SET(states.updated, SENSORUPDATES_gyro)) {
gyroRaw[0] = states.gyro[0];
gyroRaw[1] = states.gyro[1];
gyroRaw[2] = states.gyro[2];
}
FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_3_DIMENSIONS(AccelSensor, accel, x, y, z); FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_3_DIMENSIONS(AccelSensor, accel, x, y, z);
FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_3_DIMENSIONS(MagSensor, mag, x, y, z); FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_3_DIMENSIONS(MagSensor, mag, x, y, z);
FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_3_DIMENSIONS(GPSVelocitySensor, vel, North, East, Down); FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_3_DIMENSIONS(GPSVelocitySensor, vel, North, East, Down);
@ -453,7 +462,12 @@ static void StateEstimationCb(void)
case RUNSTATE_SAVE: case RUNSTATE_SAVE:
// the final output of filters is saved in state variables // the final output of filters is saved in state variables
EXPORT_STATE_TO_UAVOBJECT_IF_UPDATED_3_DIMENSIONS(GyroState, gyro, x, y, z); // EXPORT_STATE_TO_UAVOBJECT_IF_UPDATED_3_DIMENSIONS(GyroState, gyro, x, y, z) // replaced by performance shortcut
if (IS_SET(states.updated, SENSORUPDATES_gyro)) {
gyroDelta[0] = states.gyro[0] - gyroRaw[0];
gyroDelta[1] = states.gyro[1] - gyroRaw[1];
gyroDelta[2] = states.gyro[2] - gyroRaw[2];
}
EXPORT_STATE_TO_UAVOBJECT_IF_UPDATED_3_DIMENSIONS(AccelState, accel, x, y, z); EXPORT_STATE_TO_UAVOBJECT_IF_UPDATED_3_DIMENSIONS(AccelState, accel, x, y, z);
EXPORT_STATE_TO_UAVOBJECT_IF_UPDATED_3_DIMENSIONS(MagState, mag, x, y, z); EXPORT_STATE_TO_UAVOBJECT_IF_UPDATED_3_DIMENSIONS(MagState, mag, x, y, z);
EXPORT_STATE_TO_UAVOBJECT_IF_UPDATED_3_DIMENSIONS(PositionState, pos, North, East, Down); EXPORT_STATE_TO_UAVOBJECT_IF_UPDATED_3_DIMENSIONS(PositionState, pos, North, East, Down);
@ -540,6 +554,14 @@ static void sensorUpdatedCb(UAVObjEvent *ev)
if (ev->obj == GyroSensorHandle()) { if (ev->obj == GyroSensorHandle()) {
updatedSensors |= SENSORUPDATES_gyro; updatedSensors |= SENSORUPDATES_gyro;
// shortcut - update GyroState right away
GyroSensorData s;
GyroStateData t;
GyroSensorGet(&s);
t.x = s.x + gyroDelta[0];
t.y = s.y + gyroDelta[1];
t.z = s.z + gyroDelta[2];
GyroStateSet(&t);
} }
if (ev->obj == AccelSensorHandle()) { if (ev->obj == AccelSensorHandle()) {