coolfactor/quadrotor
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

Limit integral windup, some new PID values and linearise thrust.

Browse Source
This commit is contained in:
Gavan Fantom
2012-12-08 12:31:17 +00:00
parent 999e129e2c
commit 274805c2a0
5 changed files with 75 additions and 23 deletions
Download Patch File Download Diff File Expand all files Collapse all files

30
motor.c
View File

@@ -1,7 +1,7 @@
/* motor.c */
#include "stick.h"
#include "timer.h"
#include "thrust.h"
#include "dcm.h"
#include "uart.h"
#include "status.h"
@@ -11,8 +11,8 @@ float last[3];
float throttle = 0.0f;
#define Kp 0.2
#define Ki 0.04
#define Kp 0.3
#define Ki 0.02
#define Kd 0.08
#define Ka 0.0
@@ -57,6 +57,14 @@ void motor_pid_update(float troll, float mroll,
integral[1] += pitch * delta_t;
integral[2] += yaw * delta_t;
#define INTEGRAL_LIMIT 1.0f
for (i = 0; i < 3; i++) {
if (integral[i] > INTEGRAL_LIMIT)
integral[i] = INTEGRAL_LIMIT;
if (integral[i] < -INTEGRAL_LIMIT)
integral[i] = -INTEGRAL_LIMIT;
}
/* The measurements are the opposite sign to the error */
derivative[0] = (-mroll - last[0]) / delta_t;
derivative[1] = (-mpitch - last[1]) / delta_t;
@@ -133,18 +141,18 @@ void motor_pid_update(float troll, float mroll,
}
}
timer_set_pwm_value(0, (int)(motor[0] * PWM_MAX));
timer_set_pwm_value(1, (int)(motor[1] * PWM_MAX));
timer_set_pwm_value(2, (int)(motor[2] * PWM_MAX));
timer_set_pwm_value(3, (int)(motor[3] * PWM_MAX));
set_thrust(0, motor[0]);
set_thrust(1, motor[1]);
set_thrust(2, motor[2]);
set_thrust(3, motor[3]);
}
void motor_kill(void) {
throttle = 0.0;
timer_set_pwm_value(0, 0);
timer_set_pwm_value(1, 0);
timer_set_pwm_value(2, 0);
timer_set_pwm_value(3, 0);
set_thrust(0, 0.0);
set_thrust(1, 0.0);
set_thrust(2, 0.0);
set_thrust(3, 0.0);
}
void motor_set_throttle(float t) {