Commit a4901e09 authored by Ian McInerney's avatar Ian McInerney

Working LQI controller

parent 5431bc0e
......@@ -28,7 +28,7 @@ SENSORS ?= stock
ESTIMATOR ?= complementary
COMPUTATION ?= localization
#CONTROLLER ?= pid
CONTROLLER ?= lqr
CONTROLLER ?= lqi
POWER_DISTRIBUTION ?= stock
......
......@@ -29,6 +29,8 @@
#include "stabilizer_types.h"
#include "crtp.h"
#include <math.h>
enum STATE_CONTROLLER_TYPE {
CONTROLLER_BYPASS = 0,
CONTROLLER_PID = 1,
......@@ -51,6 +53,20 @@ inline float controller_thrustAdjustment(float batteryVolt) {
return( scale );
}
/**
* Pseudo-nonlinear correction to correct the X & Y for a non zero yaw
*
* @param correctedX Pointer to the variable to store the body-frame X
* @param correctedY Pointer to the variable to store the body-frame Y
* @param x The earth-frame X location
* @param y The earth-frame Y location
* @param yaw The yaw angle in radians
*/
inline void controller_yawCorrection(float *correctedX, float *correctedY, float x, float y, float yaw) {
*correctedX = (x*cosf(yaw)) - (y*sinf(yaw));
*correctedY = (x*sinf(yaw)) + (y*cosf(yaw));
}
void stateControllerInit(void);
bool stateControllerTest(void);
void stateController(control_t *control, const sensorData_t *sensors,
......
......@@ -79,12 +79,19 @@ float K[4][12] = {
{ 0.000, 0.000, 0.000, 0.000, 0.000, 1846.069, 0.000, 0.000, 0.000, 0.000, 0.000, 5000.000 }
};*/
/* These gains seem to work nicely. They do have some oscillation on the Z axis immediately after takeoff */
/* These gains seem to work nicely. They do have some oscillation on the Z axis immediately after takeoff
float K[4][12] = {
{ 0.000, 0.000, -12413.329, 0.000, 0.000, 0.000, 0.000, 0.000, -7500.000, 0.000, 0.000, 0.000 },
{ 0.000, 2803.265, 0.000, 1278.407, 0.000, 0.000, 0.000, 2500.000, 0.000, 8169.974, 0.000, 0.000 },
{ -2805.337, 0.000, 0.000, 0.000, 1284.241, 0.000, -2500.000, 0.000, 0.000, 0.000, 8192.772, 0.000 },
{ 0.000, 0.000, 0.000, 0.000, 0.000, 1911.929, 0.000, 0.000, 0.000, 0.000, 0.000, 5000.000 }
};*/
float K[4][12] = {
{ 0.000, 0.000, -16584.814, 0.000, 0.000, 0.000, 0.000, 0.000, -7500.000, 0.000, 0.000, 0.000 },
{ 0.000, 3251.943, 0.000, 1365.118, 0.000, 0.000, 0.000, 2500.000, 0.000, 9085.910, 0.000, 0.000 },
{ -3253.849, 0.000, 0.000, 0.000, 1371.253, 0.000, -2500.000, 0.000, 0.000, 0.000, 9110.242, 0.000 },
{ 0.000, 0.000, 0.000, 0.000, 0.000, 1927.848, 0.000, 0.000, 0.000, 0.000, 0.000, 5000.000 }
};
// Array to store the calculated errors
......
Markdown is supported
0%
or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment