rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-11-30 08:24:11 +01:00
Code Issues Releases Activity
7082eda175
LibrePilot/flight/Modules/AltitudeHold/altitudehold.c

373 lines
23 KiB
C
Raw Normal View History

INS: First pass at altitude hold code
2011-09-13 07:25:03 +02:00
/**
******************************************************************************
*
* @file guidance.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief This module compared @ref PositionActuatl to @ref ActiveWaypoint
* and sets @ref AttitudeDesired. It only does this when the FlightMode field
* of @ref ManualControlCommand is Auto.
*
* @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
*/
/**
* Input object: ActiveWaypoint
* Input object: PositionActual
* Input object: ManualControlCommand
* Output object: AttitudeDesired
*
* This module will periodically update the value of the AttitudeDesired object.
*
* The module executes in its own thread in this example.
*
* Modules have no API, all communication to other modules is done through UAVObjects.
* However modules may use the API exposed by shared libraries.
* See the OpenPilot wiki for more details.
* http://www.openpilot.org/OpenPilot_Application_Architecture
*
*/
#include "openpilot.h"
Now the desired altitude is relative to when the switch was flipped so that the baro altitude module can use a smoothed altitude estimate for the starting point.
2012-02-09 06:02:29 +01:00
#include <math.h>
Add settings for the altitude kalman filter settings and init the state when first measurement arrives
2012-02-12 07:35:19 +01:00
#include "CoordinateConversions.h"
Kalman filter for altitude
2012-02-12 03:13:32 +01:00
#include "altholdsmoothed.h"
Add settings for the altitude kalman filter settings and init the state when first measurement arrives
2012-02-12 07:35:19 +01:00
#include "attitudeactual.h"
INS: First pass at altitude hold code
2011-09-13 07:25:03 +02:00
#include "altitudeholdsettings.h"
#include "altitudeholddesired.h" // object that will be updated by the module
Rework the altitude hold code a bit
2012-02-08 16:42:10 +01:00
#include "baroaltitude.h"
INS: First pass at altitude hold code
2011-09-13 07:25:03 +02:00
#include "positionactual.h"
Rework the altitude hold code a bit
2012-02-08 16:42:10 +01:00
#include "flightstatus.h"
INS: First pass at altitude hold code
2011-09-13 07:25:03 +02:00
#include "stabilizationdesired.h"
Kalman filter for altitude
2012-02-12 03:13:32 +01:00
#include "accels.h"
INS: First pass at altitude hold code
2011-09-13 07:25:03 +02:00
// Private constants
Now the desired altitude is relative to when the switch was flipped so that the baro altitude module can use a smoothed altitude estimate for the starting point.
2012-02-09 06:02:29 +01:00
#define MAX_QUEUE_SIZE 2
INS: First pass at altitude hold code
2011-09-13 07:25:03 +02:00
#define STACK_SIZE_BYTES 1024
Now the desired altitude is relative to when the switch was flipped so that the baro altitude module can use a smoothed altitude estimate for the starting point.
2012-02-09 06:02:29 +01:00
#define TASK_PRIORITY (tskIDLE_PRIORITY+1)
INS: First pass at altitude hold code
2011-09-13 07:25:03 +02:00
// Private types
// Private variables
static xTaskHandle altitudeHoldTaskHandle;
static xQueueHandle queue;
Rework the altitude hold code a bit
2012-02-08 16:42:10 +01:00
static AltitudeHoldSettingsData altitudeHoldSettings;
INS: First pass at altitude hold code
2011-09-13 07:25:03 +02:00
// Private functions
static void altitudeHoldTask(void *parameters);
Add derivative term
2012-01-03 00:19:14 +01:00
static void SettingsUpdatedCb(UAVObjEvent * ev);
INS: First pass at altitude hold code
2011-09-13 07:25:03 +02:00
/**
* Initialise the module, called on startup
* \returns 0 on success or -1 if initialisation failed
*/
int32_t AltitudeHoldStart()
{
// Start main task
xTaskCreate(altitudeHoldTask, (signed char *)"AltitudeHold", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &altitudeHoldTaskHandle);
Update task info to cover the AltitudeHold module
2012-03-10 18:14:30 +01:00
TaskMonitorAdd(TASKINFO_RUNNING_ALTITUDEHOLD, altitudeHoldTaskHandle);
INS: First pass at altitude hold code
2011-09-13 07:25:03 +02:00
return 0;
}
/**
* Initialise the module, called on startup
* \returns 0 on success or -1 if initialisation failed
*/
int32_t AltitudeHoldInitialize()
{
AltitudeHoldSettingsInitialize();
AltitudeHoldDesiredInitialize();
Kalman filter for altitude
2012-02-12 03:13:32 +01:00
AltHoldSmoothedInitialize();
INS: First pass at altitude hold code
2011-09-13 07:25:03 +02:00
// Create object queue
queue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjEvent));
Rework the altitude hold code a bit
2012-02-08 16:42:10 +01:00
Add derivative term
2012-01-03 00:19:14 +01:00
AltitudeHoldSettingsConnectCallback(&SettingsUpdatedCb);
Rework the altitude hold code a bit
2012-02-08 16:42:10 +01:00
INS: First pass at altitude hold code
2011-09-13 07:25:03 +02:00
return 0;
}
MODULE_INITCALL(AltitudeHoldInitialize, AltitudeHoldStart)
Rework the altitude hold code a bit
2012-02-08 16:42:10 +01:00
float tau;
float throttleIntegral;
Add settings for the altitude kalman filter settings and init the state when first measurement arrives
2012-02-12 07:35:19 +01:00
float velocity;
Rework the altitude hold code a bit
2012-02-08 16:42:10 +01:00
float decay;
Separately smooth the error estimate from the derivative
2012-02-08 17:56:39 +01:00
float velocity_decay;
Rework the altitude hold code a bit
2012-02-08 16:42:10 +01:00
bool running = false;
Use backtracking to limit integral in altitude hold and smooth the velocity error. Initialize the throttle integral better.
2012-02-08 17:38:04 +01:00
float error;
Switch order of some commands to make sure it loads the correct hover throttle
2012-02-09 03:47:10 +01:00
float switchThrottle;
float smoothed_altitude;
Now the desired altitude is relative to when the switch was flipped so that the baro altitude module can use a smoothed altitude estimate for the starting point.
2012-02-09 06:02:29 +01:00
float starting_altitude;
Add settings for the altitude kalman filter settings and init the state when first measurement arrives
2012-02-12 07:35:19 +01:00
INS: First pass at altitude hold code
2011-09-13 07:25:03 +02:00
/**
* Module thread, should not return.
*/
static void altitudeHoldTask(void *parameters)
{
AltitudeHoldDesiredData altitudeHoldDesired;
StabilizationDesiredData stabilizationDesired;
Rework the altitude hold code a bit
2012-02-08 16:42:10 +01:00
Get rid of some unused variables
2012-03-03 18:54:09 +01:00
portTickType thisTime, lastUpdateTime;
INS: First pass at altitude hold code
2011-09-13 07:25:03 +02:00
UAVObjEvent ev;
Disable overosync by default. It was triggering event system warnings due to the extra updates from altholdsmoothed. Increase queue size when reenabled. Also AltHold is is trigger warnings because other tasks starve it during startup.
2012-03-21 06:26:04 +01:00
Add derivative term
2012-01-03 00:19:14 +01:00
// Force update of the settings
SettingsUpdatedCb(&ev);
Rework the altitude hold code a bit
2012-02-08 16:42:10 +01:00
Disable overosync by default. It was triggering event system warnings due to the extra updates from altholdsmoothed. Increase queue size when reenabled. Also AltHold is is trigger warnings because other tasks starve it during startup.
2012-03-21 06:26:04 +01:00
// Listen for updates.
AltitudeHoldDesiredConnectQueue(queue);
BaroAltitudeConnectQueue(queue);
FlightStatusConnectQueue(queue);
AccelsConnectQueue(queue);
Now the desired altitude is relative to when the switch was flipped so that the baro altitude module can use a smoothed altitude estimate for the starting point.
2012-02-09 06:02:29 +01:00
BaroAltitudeAltitudeGet(&smoothed_altitude);
Separately smooth the error estimate from the derivative
2012-02-08 17:56:39 +01:00
running = false;
Add settings for the altitude kalman filter settings and init the state when first measurement arrives
2012-02-12 07:35:19 +01:00
enum init_state {WAITING_BARO, WAITIING_INIT, INITED} init = WAITING_BARO;
Separately smooth the error estimate from the derivative
2012-02-08 17:56:39 +01:00
INS: First pass at altitude hold code
2011-09-13 07:25:03 +02:00
// Main task loop
Initialize loop variable OUTSIDE loop so that baro is used still for altitude hold.
2012-03-10 18:28:51 +01:00
bool baro_updated = false;
Add bias estimation to the altitude fusion algorithm. Necessary to increase the gains on acceleration and velocity feedback terms (they are a problem when biased).
2012-02-19 18:34:12 +01:00
while (1) {
INS: First pass at altitude hold code
2011-09-13 07:25:03 +02:00
// Wait until the AttitudeRaw object is updated, if a timeout then go to failsafe
if ( xQueueReceive(queue, &ev, 100 / portTICK_RATE_MS) != pdTRUE )
{
Use backtracking to limit integral in altitude hold and smooth the velocity error. Initialize the throttle integral better.
2012-02-08 17:38:04 +01:00
if(!running)
throttleIntegral = 0;
INS: First pass at altitude hold code
2011-09-13 07:25:03 +02:00
// Todo: Add alarm if it should be running
continue;
Rework the altitude hold code a bit
2012-02-08 16:42:10 +01:00
} else if (ev.obj == BaroAltitudeHandle()) {
Update the altitude KF to use the updated measurements appropriately and check in the relevant matlab code to check it and genererate it. Produces quite smooth traces.
2012-02-16 12:33:47 +01:00
baro_updated = true;
Add settings for the altitude kalman filter settings and init the state when first measurement arrives
2012-02-12 07:35:19 +01:00
init = (init == WAITING_BARO) ? WAITIING_INIT : init;
Rework the altitude hold code a bit
2012-02-08 16:42:10 +01:00
} else if (ev.obj == FlightStatusHandle()) {
FlightStatusData flightStatus;
FlightStatusGet(&flightStatus);
if(flightStatus.FlightMode == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD && !running) {
// Copy the current throttle as a starting point for integral
StabilizationDesiredThrottleGet(&throttleIntegral);
Switch order of some commands to make sure it loads the correct hover throttle
2012-02-09 03:47:10 +01:00
switchThrottle = throttleIntegral;
Separately smooth the error estimate from the derivative
2012-02-08 17:56:39 +01:00
error = 0;
Switch order of some commands to make sure it loads the correct hover throttle
2012-02-09 03:47:10 +01:00
velocity = 0;
Rework the altitude hold code a bit
2012-02-08 16:42:10 +01:00
running = true;
Add settings for the altitude kalman filter settings and init the state when first measurement arrives
2012-02-12 07:35:19 +01:00
AltHoldSmoothedData altHold;
AltHoldSmoothedGet(&altHold);
starting_altitude = altHold.Altitude;
Now the desired altitude is relative to when the switch was flipped so that the baro altitude module can use a smoothed altitude estimate for the starting point.
2012-02-09 06:02:29 +01:00
} else if (flightStatus.FlightMode != FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD)
running = false;
Kalman filter for altitude
2012-02-12 03:13:32 +01:00
} else if (ev.obj == AccelsHandle()) {
Add settings for the altitude kalman filter settings and init the state when first measurement arrives
2012-02-12 07:35:19 +01:00
static uint32_t timeval;
Add bias estimation to the altitude fusion algorithm. Necessary to increase the gains on acceleration and velocity feedback terms (they are a problem when biased).
2012-02-19 18:34:12 +01:00
static float z[4] = {0, 0, 0, 0};
float z_new[4];
float P[4][4], K[4][2], x[2];
float G[4] = {1.0e-15f, 1.0e-15f, 1.0e-3f, 1.0e-7};
static float V[4][4] = {{10.0f, 0, 0, 0}, {0, 100.0f, 0, 0}, {0, 0, 100.0f, 0}, {0, 0, 0, 1000.0f}};
Add settings for the altitude kalman filter settings and init the state when first measurement arrives
2012-02-12 07:35:19 +01:00
Move the throttle setting code for alt hold into the fast accel loop.
2012-02-16 12:58:34 +01:00
float dT;
static float S[2] = {1.0f,10.0f};
thisTime = xTaskGetTickCount();
Add settings for the altitude kalman filter settings and init the state when first measurement arrives
2012-02-12 07:35:19 +01:00
/* Somehow this always assigns to zero. Compiler bug? Race condition? */
Move the throttle setting code for alt hold into the fast accel loop.
2012-02-16 12:58:34 +01:00
S[0] = altitudeHoldSettings.PressureNoise;
S[1] = altitudeHoldSettings.AccelNoise;
G[2] = altitudeHoldSettings.AccelDrift;
Kalman filter for altitude
2012-02-12 03:13:32 +01:00
AccelsData accels;
AccelsGet(&accels);
Add settings for the altitude kalman filter settings and init the state when first measurement arrives
2012-02-12 07:35:19 +01:00
AttitudeActualData attitudeActual;
AttitudeActualGet(&attitudeActual);
Kalman filter for altitude
2012-02-12 03:13:32 +01:00
BaroAltitudeData baro;
BaroAltitudeGet(&baro);
Add settings for the altitude kalman filter settings and init the state when first measurement arrives
2012-02-12 07:35:19 +01:00
if (init == WAITIING_INIT) {
z[0] = baro.Altitude;
z[1] = 0;
Add bias estimation to the altitude fusion algorithm. Necessary to increase the gains on acceleration and velocity feedback terms (they are a problem when biased).
2012-02-19 18:34:12 +01:00
z[2] = accels.z;
z[3] = 0;
Add settings for the altitude kalman filter settings and init the state when first measurement arrives
2012-02-12 07:35:19 +01:00
init = INITED;
} else if (init == WAITING_BARO)
continue;
Kalman filter for altitude
2012-02-12 03:13:32 +01:00
x[0] = baro.Altitude;
Update the altitude KF to use the updated measurements appropriately and check in the relevant matlab code to check it and genererate it. Produces quite smooth traces.
2012-02-16 12:33:47 +01:00
//rotate avg accels into earth frame and store it
if(1) {
float q[4], Rbe[3][3];
q[0]=attitudeActual.q1;
q[1]=attitudeActual.q2;
q[2]=attitudeActual.q3;
q[3]=attitudeActual.q4;
Quaternion2R(q, Rbe);
x[1] = -(Rbe[0][2]*accels.x+ Rbe[1][2]*accels.y + Rbe[2][2]*accels.z + 9.81f);
} else {
x[1] = -accels.z + 9.81f;
}
Add settings for the altitude kalman filter settings and init the state when first measurement arrives
2012-02-12 07:35:19 +01:00
dT = PIOS_DELAY_DiffuS(timeval) / 1.0e6f;
timeval = PIOS_DELAY_GetRaw();
Add bias estimation to the altitude fusion algorithm. Necessary to increase the gains on acceleration and velocity feedback terms (they are a problem when biased).
2012-02-19 18:34:12 +01:00
P[0][0] = dT*(V[0][1]+dT*V[1][1])+V[0][0]+G[0]+dT*V[1][0];
P[0][1] = dT*(V[0][2]+dT*V[1][2])+V[0][1]+dT*V[1][1];
P[0][2] = V[0][2]+dT*V[1][2];
P[0][3] = V[0][3]+dT*V[1][3];
P[1][0] = dT*(V[1][1]+dT*V[2][1])+V[1][0]+dT*V[2][0];
P[1][1] = dT*(V[1][2]+dT*V[2][2])+V[1][1]+G[1]+dT*V[2][1];
P[1][2] = V[1][2]+dT*V[2][2];
P[1][3] = V[1][3]+dT*V[2][3];
P[2][0] = V[2][0]+dT*V[2][1];
P[2][1] = V[2][1]+dT*V[2][2];
P[2][2] = V[2][2]+G[2];
P[2][3] = V[2][3];
P[3][0] = V[3][0]+dT*V[3][1];
P[3][1] = V[3][1]+dT*V[3][2];
P[3][2] = V[3][2];
P[3][3] = V[3][3]+G[3];
Update the altitude KF to use the updated measurements appropriately and check in the relevant matlab code to check it and genererate it. Produces quite smooth traces.
2012-02-16 12:33:47 +01:00
if (baro_updated) {
Add bias estimation to the altitude fusion algorithm. Necessary to increase the gains on acceleration and velocity feedback terms (they are a problem when biased).
2012-02-19 18:34:12 +01:00
K[0][0] = -(V[2][2]*S[0]+V[2][3]*S[0]+V[3][2]*S[0]+V[3][3]*S[0]+G[2]*S[0]+G[3]*S[0]+S[0]*S[1])/(V[0][0]*G[2]+V[0][0]*G[3]+V[2][2]*G[0]+V[2][3]*G[0]+V[3][2]*G[0]+V[3][3]*G[0]+V[0][0]*S[1]+V[2][2]*S[0]+V[2][3]*S[0]+V[3][2]*S[0]+V[3][3]*S[0]+V[0][0]*V[2][2]-V[0][2]*V[2][0]+V[0][0]*V[2][3]+V[0][0]*V[3][2]-V[0][2]*V[3][0]-V[2][0]*V[0][3]+V[0][0]*V[3][3]-V[0][3]*V[3][0]+G[0]*G[2]+G[0]*G[3]+G[0]*S[1]+G[2]*S[0]+G[3]*S[0]+S[0]*S[1]+(dT*dT)*V[1][1]*V[2][2]-(dT*dT)*V[1][2]*V[2][1]+(dT*dT)*V[1][1]*V[2][3]+(dT*dT)*V[1][1]*V[3][2]-(dT*dT)*V[1][2]*V[3][1]-(dT*dT)*V[2][1]*V[1][3]+(dT*dT)*V[1][1]*V[3][3]-(dT*dT)*V[1][3]*V[3][1]+dT*V[0][1]*G[2]+dT*V[1][0]*G[2]+dT*V[0][1]*G[3]+dT*V[1][0]*G[3]+dT*V[0][1]*S[1]+dT*V[1][0]*S[1]+(dT*dT)*V[1][1]*G[2]+(dT*dT)*V[1][1]*G[3]+(dT*dT)*V[1][1]*S[1]+dT*V[0][1]*V[2][2]+dT*V[1][0]*V[2][2]-dT*V[0][2]*V[2][1]-dT*V[2][0]*V[1][2]+dT*V[0][1]*V[2][3]+dT*V[0][1]*V[3][2]+dT*V[1][0]*V[2][3]+dT*V[1][0]*V[3][2]-dT*V[0][2]*V[3][1]-dT*V[2][0]*V[1][3]-dT*V[0][3]*V[2][1]-dT*V[1][2]*V[3][0]+dT*V[0][1]*V[3][3]+dT*V[1][0]*V[3][3]-dT*V[0][3]*V[3][1]-dT*V[3][0]*V[1][3])+1.0f;
K[0][1] = ((V[0][2]+V[0][3])*S[0]+dT*(V[1][2]+V[1][3])*S[0])/(V[0][0]*G[2]+V[0][0]*G[3]+V[2][2]*G[0]+V[2][3]*G[0]+V[3][2]*G[0]+V[3][3]*G[0]+V[0][0]*S[1]+V[2][2]*S[0]+V[2][3]*S[0]+V[3][2]*S[0]+V[3][3]*S[0]+V[0][0]*V[2][2]-V[0][2]*V[2][0]+V[0][0]*V[2][3]+V[0][0]*V[3][2]-V[0][2]*V[3][0]-V[2][0]*V[0][3]+V[0][0]*V[3][3]-V[0][3]*V[3][0]+G[0]*G[2]+G[0]*G[3]+G[0]*S[1]+G[2]*S[0]+G[3]*S[0]+S[0]*S[1]+(dT*dT)*V[1][1]*V[2][2]-(dT*dT)*V[1][2]*V[2][1]+(dT*dT)*V[1][1]*V[2][3]+(dT*dT)*V[1][1]*V[3][2]-(dT*dT)*V[1][2]*V[3][1]-(dT*dT)*V[2][1]*V[1][3]+(dT*dT)*V[1][1]*V[3][3]-(dT*dT)*V[1][3]*V[3][1]+dT*V[0][1]*G[2]+dT*V[1][0]*G[2]+dT*V[0][1]*G[3]+dT*V[1][0]*G[3]+dT*V[0][1]*S[1]+dT*V[1][0]*S[1]+(dT*dT)*V[1][1]*G[2]+(dT*dT)*V[1][1]*G[3]+(dT*dT)*V[1][1]*S[1]+dT*V[0][1]*V[2][2]+dT*V[1][0]*V[2][2]-dT*V[0][2]*V[2][1]-dT*V[2][0]*V[1][2]+dT*V[0][1]*V[2][3]+dT*V[0][1]*V[3][2]+dT*V[1][0]*V[2][3]+dT*V[1][0]*V[3][2]-dT*V[0][2]*V[3][1]-dT*V[2][0]*V[1][3]-dT*V[0][3]*V[2][1]-dT*V[1][2]*V[3][0]+dT*V[0][1]*V[3][3]+dT*V[1][0]*V[3][3]-dT*V[0][3]*V[3][1]-dT*V[3][0]*V[1][3]);
K[1][0] = (V[1][0]*G[2]+V[1][0]*G[3]+V[1][0]*S[1]+V[1][0]*V[2][2]-V[2][0]*V[1][2]+V[1][0]*V[2][3]+V[1][0]*V[3][2]-V[2][0]*V[1][3]-V[1][2]*V[3][0]+V[1][0]*V[3][3]-V[3][0]*V[1][3]+(dT*dT)*V[2][1]*V[3][2]-(dT*dT)*V[2][2]*V[3][1]+(dT*dT)*V[2][1]*V[3][3]-(dT*dT)*V[3][1]*V[2][3]+dT*V[1][1]*G[2]+dT*V[2][0]*G[2]+dT*V[1][1]*G[3]+dT*V[2][0]*G[3]+dT*V[1][1]*S[1]+dT*V[2][0]*S[1]+(dT*dT)*V[2][1]*G[2]+(dT*dT)*V[2][1]*G[3]+(dT*dT)*V[2][1]*S[1]+dT*V[1][1]*V[2][2]-dT*V[1][2]*V[2][1]+dT*V[1][1]*V[2][3]+dT*V[1][1]*V[3][2]+dT*V[2][0]*V[3][2]-dT*V[1][2]*V[3][1]-dT*V[2][1]*V[1][3]-dT*V[3][0]*V[2][2]+dT*V[1][1]*V[3][3]+dT*V[2][0]*V[3][3]-dT*V[3][0]*V[2][3]-dT*V[1][3]*V[3][1])/(V[0][0]*G[2]+V[0][0]*G[3]+V[2][2]*G[0]+V[2][3]*G[0]+V[3][2]*G[0]+V[3][3]*G[0]+V[0][0]*S[1]+V[2][2]*S[0]+V[2][3]*S[0]+V[3][2]*S[0]+V[3][3]*S[0]+V[0][0]*V[2][2]-V[0][2]*V[2][0]+V[0][0]*V[2][3]+V[0][0]*V[3][2]-V[0][2]*V[3][0]-V[2][0]*V[0][3]+V[0][0]*V[3][3]-V[0][3]*V[3][0]+G[0]*G[2]+G[0]*G[3]+G[0]*S[1]+G[2]*S[0]+G[3]*S[0]+S[0]*S[1]+(dT*dT)*V[1][1]*V[2][2]-(dT*dT)*V[1][2]*V[2][1]+(dT*dT)*V[1][1]*V[2][3]+(dT*dT)*V[1][1]*V[3][2]-(dT*dT)*V[1][2]*V[3][1]-(dT*dT)*V[2][1]*V[1][3]+(dT*dT)*V[1][1]*V[3][3]-(dT*dT)*V[1][3]*V[3][1]+dT*V[0][1]*G[2]+dT*V[1][0]*G[2]+dT*V[0][1]*G[3]+dT*V[1][0]*G[3]+dT*V[0][1]*S[1]+dT*V[1][0]*S[1]+(dT*dT)*V[1][1]*G[2]+(dT*dT)*V[1][1]*G[3]+(dT*dT)*V[1][1]*S[1]+dT*V[0][1]*V[2][2]+dT*V[1][0]*V[2][2]-dT*V[0][2]*V[2][1]-dT*V[2][0]*V[1][2]+dT*V[0][1]*V[2][3]+dT*V[0][1]*V[3][2]+dT*V[1][0]*V[2][3]+dT*V[1][0]*V[3][2]-dT*V[0][2]*V[3][1]-dT*V[2][0]*V[1][3]-dT*V[0][3]*V[2][1]-dT*V[1][2]*V[3][0]+dT*V[0][1]*V[3][3]+dT*V[1][0]*V[3][3]-dT*V[0][3]*V[3][1]-dT*V[3][0]*V[1][3]);
K[1][1] = (V[1][2]*G[0]+V[1][3]*G[0]+V[1][2]*S[0]+V[1][3]*S[0]+V[0][0]*V[1][2]-V[1][0]*V[0][2]+V[0][0]*V[1][3]-V[1][0]*V[0][3]+(dT*dT)*V[0][1]*V[2][2]+(dT*dT)*V[1][0]*V[2][2]-(dT*dT)*V[0][2]*V[2][1]-(dT*dT)*V[2][0]*V[1][2]+(dT*dT)*V[0][1]*V[2][3]+(dT*dT)*V[1][0]*V[2][3]-(dT*dT)*V[2][0]*V[1][3]-(dT*dT)*V[0][3]*V[2][1]+(dT*dT*dT)*V[1][1]*V[2][2]-(dT*dT*dT)*V[1][2]*V[2][1]+(dT*dT*dT)*V[1][1]*V[2][3]-(dT*dT*dT)*V[2][1]*V[1][3]+dT*V[2][2]*G[0]+dT*V[2][3]*G[0]+dT*V[2][2]*S[0]+dT*V[2][3]*S[0]+dT*V[0][0]*V[2][2]+dT*V[0][1]*V[1][2]-dT*V[0][2]*V[1][1]-dT*V[0][2]*V[2][0]+dT*V[0][0]*V[2][3]+dT*V[0][1]*V[1][3]-dT*V[1][1]*V[0][3]-dT*V[2][0]*V[0][3])/(V[0][0]*G[2]+V[0][0]*G[3]+V[2][2]*G[0]+V[2][3]*G[0]+V[3][2]*G[0]+V[3][3]*G[0]+V[0][0]*S[1]+V[2][2]*S[0]+V[2][3]*S[0]+V[3][2]*S[0]+V[3][3]*S[0]+V[0][0]*V[2][2]-V[0][2]*V[2][0]+V[0][0]*V[2][3]+V[0][0]*V[3][2]-V[0][2]*V[3][0]-V[2][0]*V[0][3]+V[0][0]*V[3][3]-V[0][3]*V[3][0]+G[0]*G[2]+G[0]*G[3]+G[0]*S[1]+G[2]*S[0]+G[3]*S[0]+S[0]*S[1]+(dT*dT)*V[1][1]*V[2][2]-(dT*dT)*V[1][2]*V[2][1]+(dT*dT)*V[1][1]*V[2][3]+(dT*dT)*V[1][1]*V[3][2]-(dT*dT)*V[1][2]*V[3][1]-(dT*dT)*V[2][1]*V[1][3]+(dT*dT)*V[1][1]*V[3][3]-(dT*dT)*V[1][3]*V[3][1]+dT*V[0][1]*G[2]+dT*V[1][0]*G[2]+dT*V[0][1]*G[3]+dT*V[1][0]*G[3]+dT*V[0][1]*S[1]+dT*V[1][0]*S[1]+(dT*dT)*V[1][1]*G[2]+(dT*dT)*V[1][1]*G[3]+(dT*dT)*V[1][1]*S[1]+dT*V[0][1]*V[2][2]+dT*V[1][0]*V[2][2]-dT*V[0][2]*V[2][1]-dT*V[2][0]*V[1][2]+dT*V[0][1]*V[2][3]+dT*V[0][1]*V[3][2]+dT*V[1][0]*V[2][3]+dT*V[1][0]*V[3][2]-dT*V[0][2]*V[3][1]-dT*V[2][0]*V[1][3]-dT*V[0][3]*V[2][1]-dT*V[1][2]*V[3][0]+dT*V[0][1]*V[3][3]+dT*V[1][0]*V[3][3]-dT*V[0][3]*V[3][1]-dT*V[3][0]*V[1][3]);
K[2][0] = (V[2][0]*G[3]-V[3][0]*G[2]+V[2][0]*S[1]+V[2][0]*V[3][2]-V[3][0]*V[2][2]+V[2][0]*V[3][3]-V[3][0]*V[2][3]+dT*V[2][1]*G[3]-dT*V[3][1]*G[2]+dT*V[2][1]*S[1]+dT*V[2][1]*V[3][2]-dT*V[2][2]*V[3][1]+dT*V[2][1]*V[3][3]-dT*V[3][1]*V[2][3])/(V[0][0]*G[2]+V[0][0]*G[3]+V[2][2]*G[0]+V[2][3]*G[0]+V[3][2]*G[0]+V[3][3]*G[0]+V[0][0]*S[1]+V[2][2]*S[0]+V[2][3]*S[0]+V[3][2]*S[0]+V[3][3]*S[0]+V[0][0]*V[2][2]-V[0][2]*V[2][0]+V[0][0]*V[2][3]+V[0][0]*V[3][2]-V[0][2]*V[3][0]-V[2][0]*V[0][3]+V[0][0]*V[3][3]-V[0][3]*V[3][0]+G[0]*G[2]+G[0]*G[3]+G[0]*S[1]+G[2]*S[0]+G[3]*S[0]+S[0]*S[1]+(dT*dT)*V[1][1]*V[2][2]-(dT*dT)*V[1][2]*V[2][1]+(dT*dT)*V[1][1]*V[2][3]+(dT*dT)*V[1][1]*V[3][2]-(dT*dT)*V[1][2]*V[3][1]-(dT*dT)*V[2][1]*V[1][3]+(dT*dT)*V[1][1]*V[3][3]-(dT*dT)*V[1][3]*V[3][1]+dT*V[0][1]*G[2]+dT*V[1][0]*G[2]+dT*V[0][1]*G[3]+dT*V[1][0]*G[3]+dT*V[0][1]*S[1]+dT*V[1][0]*S[1]+(dT*dT)*V[1][1]*G[2]+(dT*dT)*V[1][1]*G[3]+(dT*dT)*V[1][1]*S[1]+dT*V[0][1]*V[2][2]+dT*V[1][0]*V[2][2]-dT*V[0][2]*V[2][1]-dT*V[2][0]*V[1][2]+dT*V[0][1]*V[2][3]+dT*V[0][1]*V[3][2]+dT*V[1][0]*V[2][3]+dT*V[1][0]*V[3][2]-dT*V[0][2]*V[3][1]-dT*V[2][0]*V[1][3]-dT*V[0][3]*V[2][1]-dT*V[1][2]*V[3][0]+dT*V[0][1]*V[3][3]+dT*V[1][0]*V[3][3]-dT*V[0][3]*V[3][1]-dT*V[3][0]*V[1][3]);
K[2][1] = (V[0][0]*G[2]+V[2][2]*G[0]+V[2][3]*G[0]+V[2][2]*S[0]+V[2][3]*S[0]+V[0][0]*V[2][2]-V[0][2]*V[2][0]+V[0][0]*V[2][3]-V[2][0]*V[0][3]+G[0]*G[2]+G[2]*S[0]+(dT*dT)*V[1][1]*V[2][2]-(dT*dT)*V[1][2]*V[2][1]+(dT*dT)*V[1][1]*V[2][3]-(dT*dT)*V[2][1]*V[1][3]+dT*V[0][1]*G[2]+dT*V[1][0]*G[2]+(dT*dT)*V[1][1]*G[2]+dT*V[0][1]*V[2][2]+dT*V[1][0]*V[2][2]-dT*V[0][2]*V[2][1]-dT*V[2][0]*V[1][2]+dT*V[0][1]*V[2][3]+dT*V[1][0]*V[2][3]-dT*V[2][0]*V[1][3]-dT*V[0][3]*V[2][1])/(V[0][0]*G[2]+V[0][0]*G[3]+V[2][2]*G[0]+V[2][3]*G[0]+V[3][2]*G[0]+V[3][3]*G[0]+V[0][0]*S[1]+V[2][2]*S[0]+V[2][3]*S[0]+V[3][2]*S[0]+V[3][3]*S[0]+V[0][0]*V[2][2]-V[0][2]*V[2][0]+V[0][0]*V[2][3]+V[0][0]*V[3][2]-V[0][2]*V[3][0]-V[2][0]*V[0][3]+V[0][0]*V[3][3]-V[0][3]*V[3][0]+G[0]*G[2]+G[0]*G[3]+G[0]*S[1]+G[2]*S[0]+G[3]*S[0]+S[0]*S[1]+(dT*dT)*V[1][1]*V[2][2]-(dT*dT)*V[1][2]*V[2][1]+(dT*dT)*V[1][1]*V[2][3]+(dT*dT)*V[1][1]*V[3][2]-(dT*dT)*V[1][2]*V[3][1]-(dT*dT)*V[2][1]*V[1][3]+(dT*dT)*V[1][1]*V[3][3]-(dT*dT)*V[1][3]*V[3][1]+dT*V[0][1]*G[2]+dT*V[1][0]*G[2]+dT*V[0][1]*G[3]+dT*V[1][0]*G[3]+dT*V[0][1]*S[1]+dT*V[1][0]*S[1]+(dT*dT)*V[1][1]*G[2]+(dT*dT)*V[1][1]*G[3]+(dT*dT)*V[1][1]*S[1]+dT*V[0][1]*V[2][2]+dT*V[1][0]*V[2][2]-dT*V[0][2]*V[2][1]-dT*V[2][0]*V[1][2]+dT*V[0][1]*V[2][3]+dT*V[0][1]*V[3][2]+dT*V[1][0]*V[2][3]+dT*V[1][0]*V[3][2]-dT*V[0][2]*V[3][1]-dT*V[2][0]*V[1][3]-dT*V[0][3]*V[2][1]-dT*V[1][2]*V[3][0]+dT*V[0][1]*V[3][3]+dT*V[1][0]*V[3][3]-dT*V[0][3]*V[3][1]-dT*V[3][0]*V[1][3]);
K[3][0] = (-V[2][0]*G[3]+V[3][0]*G[2]+V[3][0]*S[1]-V[2][0]*V[3][2]+V[3][0]*V[2][2]-V[2][0]*V[3][3]+V[3][0]*V[2][3]-dT*V[2][1]*G[3]+dT*V[3][1]*G[2]+dT*V[3][1]*S[1]-dT*V[2][1]*V[3][2]+dT*V[2][2]*V[3][1]-dT*V[2][1]*V[3][3]+dT*V[3][1]*V[2][3])/(V[0][0]*G[2]+V[0][0]*G[3]+V[2][2]*G[0]+V[2][3]*G[0]+V[3][2]*G[0]+V[3][3]*G[0]+V[0][0]*S[1]+V[2][2]*S[0]+V[2][3]*S[0]+V[3][2]*S[0]+V[3][3]*S[0]+V[0][0]*V[2][2]-V[0][2]*V[2][0]+V[0][0]*V[2][3]+V[0][0]*V[3][2]-V[0][2]*V[3][0]-V[2][0]*V[0][3]+V[0][0]*V[3][3]-V[0][3]*V[3][0]+G[0]*G[2]+G[0]*G[3]+G[0]*S[1]+G[2]*S[0]+G[3]*S[0]+S[0]*S[1]+(dT*dT)*V[1][1]*V[2][2]-(dT*dT)*V[1][2]*V[2][1]+(dT*dT)*V[1][1]*V[2][3]+(dT*dT)*V[1][1]*V[3][2]-(dT*dT)*V[1][2]*V[3][1]-(dT*dT)*V[2][1]*V[1][3]+(dT*dT)*V[1][1]*V[3][3]-(dT*dT)*V[1][3]*V[3][1]+dT*V[0][1]*G[2]+dT*V[1][0]*G[2]+dT*V[0][1]*G[3]+dT*V[1][0]*G[3]+dT*V[0][1]*S[1]+dT*V[1][0]*S[1]+(dT*dT)*V[1][1]*G[2]+(dT*dT)*V[1][1]*G[3]+(dT*dT)*V[1][1]*S[1]+dT*V[0][1]*V[2][2]+dT*V[1][0]*V[2][2]-dT*V[0][2]*V[2][1]-dT*V[2][0]*V[1][2]+dT*V[0][1]*V[2][3]+dT*V[0][1]*V[3][2]+dT*V[1][0]*V[2][3]+dT*V[1][0]*V[3][2]-dT*V[0][2]*V[3][1]-dT*V[2][0]*V[1][3]-dT*V[0][3]*V[2][1]-dT*V[1][2]*V[3][0]+dT*V[0][1]*V[3][3]+dT*V[1][0]*V[3][3]-dT*V[0][3]*V[3][1]-dT*V[3][0]*V[1][3]);
K[3][1] = (V[0][0]*G[3]+V[3][2]*G[0]+V[3][3]*G[0]+V[3][2]*S[0]+V[3][3]*S[0]+V[0][0]*V[3][2]-V[0][2]*V[3][0]+V[0][0]*V[3][3]-V[0][3]*V[3][0]+G[0]*G[3]+G[3]*S[0]+(dT*dT)*V[1][1]*V[3][2]-(dT*dT)*V[1][2]*V[3][1]+(dT*dT)*V[1][1]*V[3][3]-(dT*dT)*V[1][3]*V[3][1]+dT*V[0][1]*G[3]+dT*V[1][0]*G[3]+(dT*dT)*V[1][1]*G[3]+dT*V[0][1]*V[3][2]+dT*V[1][0]*V[3][2]-dT*V[0][2]*V[3][1]-dT*V[1][2]*V[3][0]+dT*V[0][1]*V[3][3]+dT*V[1][0]*V[3][3]-dT*V[0][3]*V[3][1]-dT*V[3][0]*V[1][3])/(V[0][0]*G[2]+V[0][0]*G[3]+V[2][2]*G[0]+V[2][3]*G[0]+V[3][2]*G[0]+V[3][3]*G[0]+V[0][0]*S[1]+V[2][2]*S[0]+V[2][3]*S[0]+V[3][2]*S[0]+V[3][3]*S[0]+V[0][0]*V[2][2]-V[0][2]*V[2][0]+V[0][0]*V[2][3]+V[0][0]*V[3][2]-V[0][2]*V[3][0]-V[2][0]*V[0][3]+V[0][0]*V[3][3]-V[0][3]*V[3][0]+G[0]*G[2]+G[0]*G[3]+G[0]*S[1]+G[2]*S[0]+G[3]*S[0]+S[0]*S[1]+(dT*dT)*V[1][1]*V[2][2]-(dT*dT)*V[1][2]*V[2][1]+(dT*dT)*V[1][1]*V[2][3]+(dT*dT)*V[1][1]*V[3][2]-(dT*dT)*V[1][2]*V[3][1]-(dT*dT)*V[2][1]*V[1][3]+(dT*dT)*V[1][1]*V[3][3]-(dT*dT)*V[1][3]*V[3][1]+dT*V[0][1]*G[2]+dT*V[1][0]*G[2]+dT*V[0][1]*G[3]+dT*V[1][0]*G[3]+dT*V[0][1]*S[1]+dT*V[1][0]*S[1]+(dT*dT)*V[1][1]*G[2]+(dT*dT)*V[1][1]*G[3]+(dT*dT)*V[1][1]*S[1]+dT*V[0][1]*V[2][2]+dT*V[1][0]*V[2][2]-dT*V[0][2]*V[2][1]-dT*V[2][0]*V[1][2]+dT*V[0][1]*V[2][3]+dT*V[0][1]*V[3][2]+dT*V[1][0]*V[2][3]+dT*V[1][0]*V[3][2]-dT*V[0][2]*V[3][1]-dT*V[2][0]*V[1][3]-dT*V[0][3]*V[2][1]-dT*V[1][2]*V[3][0]+dT*V[0][1]*V[3][3]+dT*V[1][0]*V[3][3]-dT*V[0][3]*V[3][1]-dT*V[3][0]*V[1][3]);
z_new[0] = -K[0][0]*(dT*z[1]-x[0]+z[0])+dT*z[1]-K[0][1]*(-x[1]+z[2]+z[3])+z[0];
z_new[1] = -K[1][0]*(dT*z[1]-x[0]+z[0])+dT*z[2]-K[1][1]*(-x[1]+z[2]+z[3])+z[1];
z_new[2] = -K[2][0]*(dT*z[1]-x[0]+z[0])-K[2][1]*(-x[1]+z[2]+z[3])+z[2];
z_new[3] = -K[3][0]*(dT*z[1]-x[0]+z[0])-K[3][1]*(-x[1]+z[2]+z[3])+z[3];
Update the altitude KF to use the updated measurements appropriately and check in the relevant matlab code to check it and genererate it. Produces quite smooth traces.
2012-02-16 12:33:47 +01:00
memcpy(z, z_new, sizeof(z_new));
Add bias estimation to the altitude fusion algorithm. Necessary to increase the gains on acceleration and velocity feedback terms (they are a problem when biased).
2012-02-19 18:34:12 +01:00
V[0][0] = -K[0][1]*P[2][0]-K[0][1]*P[3][0]-P[0][0]*(K[0][0]-1.0f);
V[0][1] = -K[0][1]*P[2][1]-K[0][1]*P[3][2]-P[0][1]*(K[0][0]-1.0f);
V[0][2] = -K[0][1]*P[2][2]-K[0][1]*P[3][2]-P[0][2]*(K[0][0]-1.0f);
V[0][3] = -K[0][1]*P[2][3]-K[0][1]*P[3][3]-P[0][3]*(K[0][0]-1.0f);
V[1][0] = P[1][0]-K[1][0]*P[0][0]-K[1][1]*P[2][0]-K[1][1]*P[3][0];
V[1][1] = P[1][1]-K[1][0]*P[0][1]-K[1][1]*P[2][1]-K[1][1]*P[3][2];
V[1][2] = P[1][2]-K[1][0]*P[0][2]-K[1][1]*P[2][2]-K[1][1]*P[3][2];
V[1][3] = P[1][3]-K[1][0]*P[0][3]-K[1][1]*P[2][3]-K[1][1]*P[3][3];
V[2][0] = -K[2][0]*P[0][0]-K[2][1]*P[3][0]-P[2][0]*(K[2][1]-1.0f);
V[2][1] = -K[2][0]*P[0][1]-K[2][1]*P[3][2]-P[2][1]*(K[2][1]-1.0f);
V[2][2] = -K[2][0]*P[0][2]-K[2][1]*P[3][2]-P[2][2]*(K[2][1]-1.0f);
V[2][3] = -K[2][0]*P[0][3]-K[2][1]*P[3][3]-P[2][3]*(K[2][1]-1.0f);
V[3][0] = -K[3][0]*P[0][0]-K[3][1]*P[2][0]-P[3][0]*(K[3][1]-1.0f);
V[3][1] = -K[3][0]*P[0][1]-K[3][1]*P[2][1]-P[3][2]*(K[3][1]-1.0f);
V[3][2] = -K[3][0]*P[0][2]-K[3][1]*P[2][2]-P[3][2]*(K[3][1]-1.0f);
V[3][3] = -K[3][0]*P[0][3]-K[3][1]*P[2][3]-P[3][3]*(K[3][1]-1.0f);
Kalman filter for altitude
2012-02-12 03:13:32 +01:00
Update the altitude KF to use the updated measurements appropriately and check in the relevant matlab code to check it and genererate it. Produces quite smooth traces.
2012-02-16 12:33:47 +01:00
baro_updated = false;
} else {
Add bias estimation to the altitude fusion algorithm. Necessary to increase the gains on acceleration and velocity feedback terms (they are a problem when biased).
2012-02-19 18:34:12 +01:00
K[0][0] = (V[0][2]+V[0][3]+dT*V[1][2]+dT*V[1][3])/(V[2][2]+V[2][3]+V[3][2]+V[3][3]+G[2]+G[3]+S[1]);
K[1][0] = (V[1][2]+V[1][3]+dT*V[2][2]+dT*V[2][3])/(V[2][2]+V[2][3]+V[3][2]+V[3][3]+G[2]+G[3]+S[1]);
K[2][0] = (V[2][2]+V[2][3]+G[2])/(V[2][2]+V[2][3]+V[3][2]+V[3][3]+G[2]+G[3]+S[1]);
K[3][0] = (V[3][2]+V[3][3]+G[3])/(V[2][2]+V[2][3]+V[3][2]+V[3][3]+G[2]+G[3]+S[1]);
Kalman filter for altitude
2012-02-12 03:13:32 +01:00
Add bias estimation to the altitude fusion algorithm. Necessary to increase the gains on acceleration and velocity feedback terms (they are a problem when biased).
2012-02-19 18:34:12 +01:00
z_new[0] = dT*z[1]-K[0][0]*(-x[1]+z[2]+z[3])+z[0];
z_new[1] = dT*z[2]-K[1][0]*(-x[1]+z[2]+z[3])+z[1];
z_new[2] = -K[2][0]*(-x[1]+z[2]+z[3])+z[2];
z_new[3] = -K[3][0]*(-x[1]+z[2]+z[3])+z[3];
Update the altitude KF to use the updated measurements appropriately and check in the relevant matlab code to check it and genererate it. Produces quite smooth traces.
2012-02-16 12:33:47 +01:00
memcpy(z, z_new, sizeof(z_new));
Add bias estimation to the altitude fusion algorithm. Necessary to increase the gains on acceleration and velocity feedback terms (they are a problem when biased).
2012-02-19 18:34:12 +01:00
V[0][0] = P[0][0]-K[0][0]*P[2][0]-K[0][0]*P[3][0];
V[0][1] = P[0][1]-K[0][0]*P[2][1]-K[0][0]*P[3][2];
V[0][2] = P[0][2]-K[0][0]*P[2][2]-K[0][0]*P[3][2];
V[0][3] = P[0][3]-K[0][0]*P[2][3]-K[0][0]*P[3][3];
V[1][0] = P[1][0]-K[1][0]*P[2][0]-K[1][0]*P[3][0];
V[1][1] = P[1][1]-K[1][0]*P[2][1]-K[1][0]*P[3][2];
V[1][2] = P[1][2]-K[1][0]*P[2][2]-K[1][0]*P[3][2];
V[1][3] = P[1][3]-K[1][0]*P[2][3]-K[1][0]*P[3][3];
V[2][0] = -K[2][0]*P[3][0]-P[2][0]*(K[2][0]-1.0f);
V[2][1] = -K[2][0]*P[3][2]-P[2][1]*(K[2][0]-1.0f);
V[2][2] = -K[2][0]*P[3][2]-P[2][2]*(K[2][0]-1.0f);
V[2][3] = -K[2][0]*P[3][3]-P[2][3]*(K[2][0]-1.0f);
V[3][0] = -K[3][0]*P[2][0]-P[3][0]*(K[3][0]-1.0f);
V[3][1] = -K[3][0]*P[2][1]-P[3][2]*(K[3][0]-1.0f);
V[3][2] = -K[3][0]*P[2][2]-P[3][2]*(K[3][0]-1.0f);
V[3][3] = -K[3][0]*P[2][3]-P[3][3]*(K[3][0]-1.0f);
Update the altitude KF to use the updated measurements appropriately and check in the relevant matlab code to check it and genererate it. Produces quite smooth traces.
2012-02-16 12:33:47 +01:00
}
Kalman filter for altitude
2012-02-12 03:13:32 +01:00
AltHoldSmoothedData altHold;
AltHoldSmoothedGet(&altHold);
altHold.Altitude = z[0];
altHold.Velocity = z[1];
altHold.Accel = z[2];
AltHoldSmoothedSet(&altHold);
Rework the altitude hold code a bit
2012-02-08 16:42:10 +01:00
Move the throttle setting code for alt hold into the fast accel loop.
2012-02-16 12:58:34 +01:00
AltHoldSmoothedGet(&altHold);
// Verify that we are in altitude hold mode
FlightStatusData flightStatus;
FlightStatusGet(&flightStatus);
if(flightStatus.FlightMode != FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD) {
running = false;
}
if (!running)
continue;
// Compute the altitude error
error = (starting_altitude + altitudeHoldDesired.Altitude) - altHold.Altitude;
// Compute integral off altitude error
Fix bug in the integral for altitude hold where it was accumulating wrong variable.
2012-02-21 07:37:52 +01:00
throttleIntegral += error * altitudeHoldSettings.Ki * dT;
Move the throttle setting code for alt hold into the fast accel loop.
2012-02-16 12:58:34 +01:00
// Only update stabilizationDesired less frequently
if((thisTime - lastUpdateTime) < 20)
continue;
lastUpdateTime = thisTime;
// Instead of explicit limit on integral you output limit feedback
StabilizationDesiredGet(&stabilizationDesired);
stabilizationDesired.Throttle = error * altitudeHoldSettings.Kp + throttleIntegral -
altHold.Velocity * altitudeHoldSettings.Kd - altHold.Accel * altitudeHoldSettings.Ka;
if(stabilizationDesired.Throttle > 1) {
throttleIntegral -= (stabilizationDesired.Throttle - 1);
stabilizationDesired.Throttle = 1;
}
else if (stabilizationDesired.Throttle < 0) {
throttleIntegral -= stabilizationDesired.Throttle;
stabilizationDesired.Throttle = 0;
}
stabilizationDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabilizationDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabilizationDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK;
stabilizationDesired.Roll = altitudeHoldDesired.Roll;
stabilizationDesired.Pitch = altitudeHoldDesired.Pitch;
stabilizationDesired.Yaw = altitudeHoldDesired.Yaw;
StabilizationDesiredSet(&stabilizationDesired);
Rework the altitude hold code a bit
2012-02-08 16:42:10 +01:00
} else if (ev.obj == AltitudeHoldDesiredHandle()) {
AltitudeHoldDesiredGet(&altitudeHoldDesired);
INS: First pass at altitude hold code
2011-09-13 07:25:03 +02:00
}
Now the desired altitude is relative to when the switch was flipped so that the baro altitude module can use a smoothed altitude estimate for the starting point.
2012-02-09 06:02:29 +01:00
INS: First pass at altitude hold code
2011-09-13 07:25:03 +02:00
}
}
Add derivative term
2012-01-03 00:19:14 +01:00
static void SettingsUpdatedCb(UAVObjEvent * ev)
{
AltitudeHoldSettingsGet(&altitudeHoldSettings);
}
Reference in New Issue Copy Permalink