Commit f58c756c authored by Ian McInerney's avatar Ian McInerney

New LQR controller gains

parent b28daaee
......@@ -7,6 +7,7 @@
#include "sensfusion6.h"
#include "pm.h"
#include "log.h"
#include "param.h"
#include "crtp.h"
......@@ -39,11 +40,51 @@ const int stateControllerType = CONTROLLER_LQR;
#define USE_PSEUDO_NONLINEAR_CORRECTION
/* This is a very aggressive LQR, and is very oscillatory on X & Y (100 weight on X & Y)
float K[4][12] = {
{ 0.000, 0.000, -6362.910, 0.000, 0.000, 0.000, 0.000, 0.000, -5000.000, 0.000, 0.000, 0.000 },
{ 0.000, 5109.467, 0.000, 1636.731, 0.000, 0.000, 0.000, 10000.000, 0.000, 12809.304, 0.000, 0.000 },
{ -5117.338, 0.000, 0.000, 0.000, 1644.304, 0.000, -10000.000, 0.000, 0.000, 0.000, 12848.790, 0.000 },
{ 0.000, 0.000, 0.000, 0.000, 0.000, 1822.820, 0.000, 0.000, 0.000, 0.000, 0.000, 5000.000 }
};*/
/* This is somewhat oscillatory on X & Y (50 weight on X & Y)
float K[4][12] = {
{ 0.000, 0.000, -6362.910, 0.000, 0.000, 0.000, 0.000, 0.000, -5000.000, 0.000, 0.000, 0.000 },
{ 0.000, 3037.715, 0.000, 1376.145, 0.000, 0.000, 0.000, 5000.000, 0.000, 9056.373, 0.000, 0.000 },
{ -3042.397, 0.000, 0.000, 0.000, 1382.513, 0.000, -5000.000, 0.000, 0.000, 0.000, 9084.294, 0.000 },
{ 0.000, 0.000, 0.000, 0.000, 0.000, 1822.820, 0.000, 0.000, 0.000, 0.000, 0.000, 5000.000 }
};*/
/* This is still oscillatory on X & Y (30 weight on X & Y)
float K[4][12] = {
{ 0.000, 0.000, -6005.247, 0.000, 0.000, 0.000, 0.000, 0.000, -5000.000, 0.000, 0.000, 0.000 },
{ 0.000, 2071.112, 0.000, 1226.387, 0.000, 0.000, 0.000, 3000.000, 0.000, 7059.255, 0.000, 0.000 },
{ -2074.344, 0.000, 0.000, 0.000, 1232.087, 0.000, -3000.000, 0.000, 0.000, 0.000, 7081.161, 0.000 },
{ 0.000, 0.000, 0.000, 0.000, 0.000, 1846.069, 0.000, 0.000, 0.000, 0.000, 0.000, 5000.000 }
};*/
/* (20 weight on X & Y)
float K[4][12] = {
{ 0.000, 0.000, -6005.247, 0.000, 0.000, 0.000, 0.000, 0.000, -5000.000, 0.000, 0.000, 0.000 },
{ 0.000, 1526.125, 0.000, 1106.506, 0.000, 0.000, 0.000, 2000.000, 0.000, 5755.885, 0.000, 0.000 },
{ -1528.513, 0.000, 0.000, 0.000, 1111.654, 0.000, -2000.000, 0.000, 0.000, 0.000, 5773.771, 0.000 },
{ 0.000, 0.000, 0.000, 0.000, 0.000, 1846.069, 0.000, 0.000, 0.000, 0.000, 0.000, 5000.000 }
};*/
/* (20 weight on X & Y, 4 weight on angles)
float K[4][12] = {
{ 0.000, 0.000, -6005.247, 0.000, 0.000, 0.000, 0.000, 0.000, -5000.000, 0.000, 0.000, 0.000 },
{ 0.000, 1429.607, 0.000, 1036.449, 0.000, 0.000, 0.000, 2000.000, 0.000, 5406.229, 0.000, 0.000 },
{ -1432.155, 0.000, 0.000, 0.000, 1041.498, 0.000, -2000.000, 0.000, 0.000, 0.000, 5424.114, 0.000 },
{ 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 */
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 }
};
// Array to store the calculated errors
......@@ -126,6 +167,8 @@ void stateController(control_t *control, const sensorData_t *sensors,
const uint32_t tick,
const uint8_t resetControllers)
{
float scale, thrust, roll, pitch, yaw;
// If the thrust setpoint is 0, then nothing should be moving
if (setpoint->thrust == 0) {
control->thrust = 0;
......@@ -174,25 +217,33 @@ void stateController(control_t *control, const sensorData_t *sensors,
// Do the controller calculation
float controllerOutputs[4] = {0.0, 0.0, 0.0, 0.0};
for (uint8_t i = 0; i < NUM_LQR_STATES; i++) {
controllerOutputs[0] += error[i]*K[0][i];
controllerOutputs[1] += error[i]*K[1][i];
controllerOutputs[2] += error[i]*K[2][i];
controllerOutputs[3] += error[i]*K[3][i];
controllerOutputs[0] += (error[i]*K[0][i]);
controllerOutputs[1] += (error[i]*K[1][i]);
controllerOutputs[2] += (error[i]*K[2][i]);
controllerOutputs[3] += (error[i]*K[3][i]);
}
// Set the thrust controller
if (disableThrustControl) {
// No thrust control, just the setpoint thrust
control->thrust = setpoint->thrust;
thrust = setpoint->thrust;
} else {
// Thrust control active
control->thrust = setpoint->thrust + controllerOutputs[0];
thrust = setpoint->thrust + controllerOutputs[0];
}
// Set the remaining control outputs
control->roll = saturateSignedInt16(controllerOutputs[1]);
control->pitch = saturateSignedInt16(controllerOutputs[2]);
control->yaw = saturateSignedInt16(controllerOutputs[3]);
// Compensate for the battery voltage drop
scale = controller_thrustAdjustment( pmGetBatteryVoltage() );
thrust = scale * thrust;
roll = scale * ( controllerOutputs[1] );
pitch = scale * ( controllerOutputs[2] );
yaw = scale * ( controllerOutputs[3] );
// Saturate the control outputs
control->thrust = thrust;
control->roll = saturateSignedInt16( roll );
control->pitch = saturateSignedInt16( pitch );
control->yaw = saturateSignedInt16( yaw );
}
}
......
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