Browse Source

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

master
Gavan Fantom 12 years ago
parent
commit
274805c2a0
  1. 1
      Makefile
  2. 48
      main.c
  3. 30
      motor.c
  4. 15
      thrust.c
  5. 4
      thrust.h

1
Makefile

@ -5,6 +5,7 @@ NAME=quad
SSRCS=crt0.s
CSRCS=main.c i2c.c wmp.c timer.c interrupt.c uart.c event.c matrix.c dcm.c
CSRCS+=fisqrt.c stick.c trig.c motor.c led.c watchdog.c panic.c status.c
CSRCS+=thrust.c
#PROJOPTS=-DUSE_UART -DSEND_DCM

48
main.c

@ -9,6 +9,7 @@
#include "led.h"
#include "status.h"
#include "watchdog.h"
#include "thrust.h"
#define PINSEL0 (*((volatile unsigned int *) 0xE002C000))
#define PINSEL1 (*((volatile unsigned int *) 0xE002C004))
@ -219,6 +220,8 @@ int main(void) {
return 0;
}
static int power = 0;
void menu_handler(void)
{
int i;
@ -329,35 +332,56 @@ void menu_handler(void)
}
break;
case '0' & 0xdf:
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);
power = 0;
break;
#if 0
case '1' & 0xdf:
power--;
if (power < 0)
power = 15;
power = power % 16;
putstr("Power setting: ");
putint(power);
putstr("\r\n");
set_thrust(0, ((float)power)/16.0);
break;
case '2' & 0xdf:
power++;
power = power % 16;
putstr("Power setting: ");
putint(power);
putstr("\r\n");
set_thrust(0, ((float)power)/16.0);
break;
#endif
#if 0
case '1' & 0xdf:
timer_set_pwm_value(0, PWM_MAX/2);
set_thrust(0, 0.5);
break;
case '2' & 0xdf:
timer_set_pwm_value(1, PWM_MAX/2);
set_thrust(1, 0.5);
break;
case '3' & 0xdf:
timer_set_pwm_value(2, PWM_MAX/2);
set_thrust(2, 0.5);
break;
case '4' & 0xdf:
timer_set_pwm_value(3, PWM_MAX/2);
set_thrust(3, 0.5);
break;
case '5' & 0xdf:
timer_set_pwm_value(0, PWM_MAX);
set_thrust(0, 1.0);
break;
case '6' & 0xdf:
timer_set_pwm_value(1, PWM_MAX);
set_thrust(1, 1.0);
break;
case '7' & 0xdf:
timer_set_pwm_value(2, PWM_MAX);
set_thrust(2, 1.0);
break;
case '8' & 0xdf:
timer_set_pwm_value(3, PWM_MAX);
set_thrust(3, 1.0);
break;
#endif
case '9' & 0xdf:

30
motor.c

@ -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) {

15
thrust.c

@ -0,0 +1,15 @@
/* thrust.c */
#include "fisqrt.h"
#include "timer.h"
float linearise_thrust(float x)
{
return fisqrt(fisqrt(x*x*x));
}
void set_thrust(int motor, float x)
{
timer_set_pwm_value(motor, (int)(linearise_thrust(x) * PWM_MAX));
}

4
thrust.h

@ -0,0 +1,4 @@
/* thrust.h */
float linearise_thrust(float x);
void set_thrust(int motor, float x);
Loading…
Cancel
Save