Commit 76044bd2 authored by Ian McInerney's avatar Ian McInerney

Changed scale computation function to just be a scaling generator

parent 8cf984dd
......@@ -32,22 +32,23 @@
enum STATE_CONTROLLER_TYPE {
CONTROLLER_BYPASS = 0,
CONTROLLER_PID = 1,
CONTROLLER_LQR = 2
CONTROLLER_LQR = 2,
CONTROLLER_LQI = 3
};
extern const int stateControllerType;
/**
* Adjust the thrust value to take into account the decreased battery voltage.
* Compute the scaling value to take into account the decreased battery voltage
* when doing the controller values.
*
* @param The thrust value
* @param batteryVolt The battery voltage
* @return Adjusted thrust value
* @return Scale factor [0, 2]
*/
inline float controller_thrustAdjustment(float thrust, float batteryVolt) {
inline float controller_thrustAdjustment(float batteryVolt) {
float scale = 1 + (3.7 - batteryVolt)/3.7;
return( thrust * scale );
return( scale );
}
void stateControllerInit(void);
......
......@@ -188,7 +188,7 @@ void stateController(control_t *control, const sensorData_t *sensors,
thrust = 0;
}
control->thrust = controller_thrustAdjustment(thrust, pmGetBatteryVoltage() );
control->thrust = controller_thrustAdjustment( pmGetBatteryVoltage() ) * thrust;
}
......
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