Browse Source
loops, boot-time initialisation, option to run without attached UART. And.. it flies!master
18 changed files with 727 additions and 23 deletions
@ -0,0 +1,153 @@
|
||||
/* motor.c */ |
||||
|
||||
#include "stick.h" |
||||
#include "timer.h" |
||||
#include "dcm.h" |
||||
#include "uart.h" |
||||
|
||||
float integral[3] = {0.0f, 0.0f, 0.0f}; |
||||
float last[3]; |
||||
|
||||
float throttle = 0.0f; |
||||
|
||||
#define Kp 0.2 |
||||
#define Ki 0.04 |
||||
#define Kd 0.08 |
||||
#define Ka 0.0 |
||||
|
||||
#define Kp_y 0.2 |
||||
#define Ki_y 0.00 |
||||
#define Kd_y 0.00 |
||||
#define Ka_y 0.0 |
||||
|
||||
|
||||
/*
|
||||
* Perform a PID loop iteration. |
||||
* roll and pitch are absolute values |
||||
* yaw is, currently, a rate. |
||||
*/ |
||||
void motor_pid_update(float troll, float mroll, |
||||
float tpitch, float mpitch, |
||||
float tyaw, float myaw) |
||||
{ |
||||
float derivative[3]; |
||||
float out[3]; |
||||
float motor[3]; |
||||
float roll, pitch, yaw; |
||||
float error, max_error; |
||||
float min_motor; |
||||
int i; |
||||
|
||||
roll = troll - mroll; |
||||
pitch = tpitch - mpitch; |
||||
yaw = tyaw - myaw; |
||||
|
||||
#if 0 |
||||
if ((stick_counter % 100) == 0) { |
||||
putstr("{"); |
||||
putint_s((int)(tyaw * 10000)); |
||||
putstr(", "); |
||||
putint_s((int)(myaw * 10000)); |
||||
putstr("}\r\n"); |
||||
} |
||||
#endif |
||||
|
||||
integral[0] += roll * delta_t; |
||||
integral[1] += pitch * delta_t; |
||||
integral[2] += yaw * delta_t; |
||||
|
||||
/* The measurements are the opposite sign to the error */ |
||||
derivative[0] = (-mroll - last[0]) / delta_t; |
||||
derivative[1] = (-mpitch - last[1]) / delta_t; |
||||
derivative[2] = (-myaw - last[2]) / delta_t; |
||||
|
||||
last[0] = -mroll; |
||||
last[1] = -mpitch; |
||||
last[2] = -myaw; |
||||
|
||||
out[0] = roll * Kp + integral[0] * Ki + derivative[0] * Kd; |
||||
out[1] = pitch * Kp + integral[1] * Ki + derivative[1] * Kd; |
||||
out[2] = yaw * Kp_y + integral[2] * Ki_y + derivative[2] * Kd_y; |
||||
|
||||
if (armed) { |
||||
/* Front right */ |
||||
motor[0] = throttle + out[0] + out[1] + out[2]; |
||||
/* Front left */ |
||||
motor[1] = throttle - out[0] + out[1] - out[2]; |
||||
/* Rear left */ |
||||
motor[2] = throttle - out[0] - out[1] + out[2]; |
||||
/* Rear right */ |
||||
motor[3] = throttle + out[0] - out[1] - out[2]; |
||||
} else { |
||||
motor[0] = 0.0; |
||||
motor[1] = 0.0; |
||||
motor[2] = 0.0; |
||||
motor[3] = 0.0; |
||||
} |
||||
|
||||
max_error = 0.0; |
||||
min_motor = 1.0; |
||||
for (i = 0; i < 3; i++) { |
||||
if (motor[i] < 0.0) |
||||
motor[i] = 0.0; |
||||
if (motor[i] > 1.0f) { |
||||
error = motor[i] - 1.0f; |
||||
if (error > max_error) |
||||
max_error = error; |
||||
} |
||||
if (motor[i] < min_motor) |
||||
min_motor = motor[i]; |
||||
} |
||||
|
||||
if (max_error > 0.0) { |
||||
for (i = 0; i < 3; i++) { |
||||
motor[i] -= max_error; |
||||
if (motor[i] < 0.0) |
||||
motor[i] = 0.0; |
||||
} |
||||
} |
||||
|
||||
if (throttle <= 0.0) { |
||||
motor[0] = 0.0; |
||||
motor[1] = 0.0; |
||||
motor[2] = 0.0; |
||||
motor[3] = 0.0; |
||||
integral[0] = 0.0; |
||||
integral[1] = 0.0; |
||||
integral[2] = 0.0; |
||||
} |
||||
if (max_error < min_motor) { |
||||
float new_throttle2, new_out[3]; |
||||
new_throttle2 = (motor[0] + motor[1] + motor[2] + motor[3])/2.0; |
||||
new_out[0] = (motor[0] + motor[3] - new_throttle2)/2.0; |
||||
new_out[1] = (motor[0] + motor[1] - new_throttle2)/2.0; |
||||
new_out[2] = (motor[0] + motor[2] - new_throttle2)/2.0; |
||||
|
||||
/* Anti-windup */ |
||||
for (i = 0; i < 3; i++) { |
||||
if (new_out[i] > 1.0) |
||||
integral[i] -= (new_out[i]-1.0) * Ka; |
||||
if (new_out[i] < 0.0) |
||||
integral[i] -= (new_out[i]) * Ka; |
||||
} |
||||
} |
||||
|
||||
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)); |
||||
} |
||||
|
||||
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); |
||||
} |
||||
|
||||
void motor_set_throttle(float t) { |
||||
if (armed) |
||||
throttle = t; |
||||
} |
||||
|
||||
@ -0,0 +1,9 @@
|
||||
/* motor.h */ |
||||
|
||||
extern float temp_kp; |
||||
|
||||
void motor_pid_update(float troll, float mroll, |
||||
float tpitch, float mpitch, |
||||
float tyaw, float myaw); |
||||
void motor_kill(void); |
||||
void motor_set_throttle(float t); |
||||
@ -0,0 +1,147 @@
|
||||
/* stick.c */ |
||||
|
||||
#ifdef WE_HAVE_SQRT |
||||
#include <math.h> |
||||
#else |
||||
#include "fisqrt.h" |
||||
#endif |
||||
#include "matrix.h" |
||||
#include "stick.h" |
||||
#include "dcm.h" |
||||
#include "uart.h" |
||||
#include "timer.h" |
||||
#include "trig.h" |
||||
#include "motor.h" |
||||
#include "wmp.h" |
||||
|
||||
#define TWO_PI 6.28318531f |
||||
#define PI 3.14159265f |
||||
|
||||
#define MIN_X 15830 |
||||
#define MAX_X 28300 |
||||
#define CENTRE_X 22100 |
||||
|
||||
#define MIN_Y 18530 |
||||
#define MAX_Y 28200 |
||||
#define CENTRE_Y 22100 |
||||
|
||||
#define MIN_Z 15800 |
||||
#define MAX_Z 28304 |
||||
#define CENTRE_Z 22100 |
||||
|
||||
#define MIN_THR 16500 |
||||
#define MAX_THR 28275 |
||||
|
||||
#define MIN_REAL_THR 15830 |
||||
|
||||
#define CENTRE_ZONE 100 |
||||
|
||||
/* Full scale is a roll/pitch angle of 30 degrees from the vertical */ |
||||
#define SCALE_X (TWO_PI*30.0/360.0 / (MAX_X-CENTRE_X)) |
||||
#define SCALE_Y (TWO_PI*30.0/360.0 / (MAX_Y-CENTRE_Y)) |
||||
|
||||
/* Full scale is a complete rotation in one second */ |
||||
#define SCALE_Z (TWO_PI / (MAX_Z-CENTRE_Z)) |
||||
|
||||
/* 0 is min throttle, 1 is max throttle */ |
||||
#define SCALE_THROTTLE (1.0f/(MAX_THR - MIN_THR)) |
||||
|
||||
float stick_yaw = 0.0f; |
||||
|
||||
unsigned int stick_counter; |
||||
|
||||
bool armed = FALSE; |
||||
|
||||
void stick_update(float x, float y, float z) |
||||
{ |
||||
float tz = delta_t * z; |
||||
|
||||
stick_yaw += tz; |
||||
|
||||
if (stick_yaw <= -PI) |
||||
stick_yaw += TWO_PI; |
||||
|
||||
if (stick_yaw > PI) |
||||
stick_yaw -= TWO_PI; |
||||
|
||||
#if 0 |
||||
z = stick_yaw; |
||||
#endif |
||||
|
||||
x = sine(x); |
||||
y = sine(y); |
||||
#if 0 |
||||
z = 1.0/fisqrt(1-x*x-y*y); |
||||
#endif |
||||
|
||||
dcm_attitude_error(x, y, z); |
||||
} |
||||
|
||||
void stick_input(void) { |
||||
float x, y, z, throttle; |
||||
if (timer_allvalid()) { |
||||
x = timer_input(0); |
||||
y = timer_input(1); |
||||
throttle = timer_input(2); |
||||
z = timer_input(3); |
||||
|
||||
if (!armed) { |
||||
if ((throttle < MIN_THR) && |
||||
(x > (CENTRE_X - CENTRE_ZONE)) && |
||||
(x < (CENTRE_X + CENTRE_ZONE)) && |
||||
(y > (CENTRE_Y - CENTRE_ZONE)) && |
||||
(y < (CENTRE_Y + CENTRE_ZONE)) && |
||||
(z > (CENTRE_Z - CENTRE_ZONE)) && |
||||
(z < (CENTRE_Z + CENTRE_ZONE)) && |
||||
(wmp_zero == FALSE)) { |
||||
putstr("ARMED!!!\r\n"); |
||||
armed = TRUE; |
||||
} |
||||
|
||||
} |
||||
|
||||
x -= CENTRE_X; |
||||
y -= CENTRE_Y; |
||||
z -= CENTRE_Z; |
||||
x = x * SCALE_X; |
||||
y = y * SCALE_Y; |
||||
z = z * SCALE_Z; |
||||
throttle = (throttle - MIN_THR) * SCALE_THROTTLE; |
||||
if (throttle < 0.0) |
||||
throttle = 0.0; |
||||
} else { |
||||
x = 0.0f; |
||||
y = 0.0f; |
||||
z = 0.0f; |
||||
throttle = 0.0f; |
||||
} |
||||
|
||||
motor_set_throttle(throttle); |
||||
|
||||
/* So the controls are back to front. Let's fix that. */ |
||||
x = -x; |
||||
y = -y; |
||||
z = -z; |
||||
|
||||
#if 0 |
||||
if ((stick_counter % 100) == 0) { |
||||
putstr("["); |
||||
putint_s((int)(z * 10000)); |
||||
putstr("] "); |
||||
} |
||||
#endif |
||||
|
||||
#if 1 |
||||
stick_update(x, y, z); |
||||
#else |
||||
if ((stick_counter % 100) == 0) |
||||
stick_update(x, y, z); |
||||
#endif |
||||
|
||||
/*
|
||||
if ((stick_counter & 3) == 0) |
||||
stick_send_packet(); |
||||
*/ |
||||
stick_counter++; |
||||
} |
||||
|
||||
@ -0,0 +1,10 @@
|
||||
/* stick.h */ |
||||
|
||||
#include "types.h" |
||||
|
||||
extern bool armed; |
||||
|
||||
extern unsigned int stick_counter; |
||||
|
||||
void stick_input(void); |
||||
void stick_update(float x, float y, float omega_z); |
||||
@ -0,0 +1,44 @@
|
||||
/* trig.c */ |
||||
|
||||
/* Cosine implementation from:
|
||||
* http://www.ganssle.com/articles/atrig.htm
|
||||
*/ |
||||
|
||||
double cosine(double x) |
||||
{ |
||||
double p0,p1,p2,p3,p4,p5,y,t,absx,frac,pi2; |
||||
int quad; |
||||
p0= 0.999999999781; |
||||
p1=-0.499999993585; |
||||
p2= 0.041666636258; |
||||
p3=-0.0013888361399; |
||||
p4= 0.00002476016134; |
||||
p5=-0.00000026051495; |
||||
pi2=1.570796326794896; /* pi/2 */ |
||||
absx=x; |
||||
if (x<0) absx=-absx; /* absolute value of input */ |
||||
quad=(int) (absx/pi2); /* quadrant (0 to 3) */ |
||||
if (quad > 3) { |
||||
int q = quad & ~3; /* round down to the next multiple of 4 */ |
||||
absx = absx / (pi2 * quad); |
||||
quad -= q; |
||||
t = 0.0; /* hello, compiler warnings */ |
||||
} |
||||
frac= (absx/pi2) - quad; /* fractional part of input */ |
||||
if(quad==0) t=frac * pi2; |
||||
if(quad==1) t=(1-frac) * pi2; |
||||
if(quad==2) t=frac * pi2; |
||||
if(quad==3) t=(frac-1) * pi2; |
||||
t=t * t; |
||||
y=p0 + (p1*t) + (p2*t*t) + (p3*t*t*t) + (p4*t*t*t*t) + (p5*t*t*t*t*t); |
||||
if(quad==2 || quad==1) y=-y; /* correct sign */ |
||||
return(y); |
||||
}; |
||||
|
||||
double sine(double x) |
||||
{ |
||||
double pi2=1.570796326794896; /* pi/2 */ |
||||
x = x - pi2; |
||||
if (x < -pi2) x += pi2*4; |
||||
return cosine(x); |
||||
} |
||||
@ -0,0 +1,5 @@
|
||||
/* trig.h */ |
||||
|
||||
double cosine(double x); |
||||
double sine(double x); |
||||
|
||||
Loading…
Reference in new issue