From 6404ce86191bbf4141725e8cace6ddb3ca5a6668 Mon Sep 17 00:00:00 2001 From: Kier Davis Date: Mon, 13 Mar 2017 13:01:20 +0000 Subject: [PATCH] stuff --- src/control.c | 12 +++++++----- src/settings.h | 6 +++--- 2 files changed, 10 insertions(+), 8 deletions(-) diff --git a/src/control.c b/src/control.c index c1b41b7..777d060 100644 --- a/src/control.c +++ b/src/control.c @@ -69,20 +69,22 @@ void control_cycle(measured_state_t *measured_state, // roll_factor = desired_state->roll*0.01; // pitch_factor = desired_state->pitch*0.01; // yaw_factor = desired_state->yaw_vel*0.1; - z_factor = 0.2; - //roll_factor = 0.0; - //pitch_factor = 0.0; + z_factor = 0.0; + roll_factor = 0.0; + pitch_factor = 0.0; yaw_factor = 0.0; //t += 0.00001; - //printf("LOG %f,%f,%f\n", dt_float, z_factor, measured_state->z_vel); + printf("LOG %f,%f,%f\n", dt_float, z_factor, measured_state->z_vel); //printf("z_vel error=%f-%f zfactor=%f\n", desired_state->z_vel, measured_state->z_vel, z_factor); + //printf("r=%f p=%f\n", measured_state->roll, measured_state->pitch); + // Combine contributions to produce rotor speeds. rotor_speeds->a = 0.0 + z_factor + roll_factor - pitch_factor + yaw_factor; rotor_speeds->b = 0.0 + z_factor - roll_factor - pitch_factor - yaw_factor; - rotor_speeds->c = 0.0 + z_factor - roll_factor + pitch_factor + yaw_factor; + rotor_speeds->c = 0.0 + z_factor - roll_factor + pitch_factor + yaw_factor - 0.02; rotor_speeds->d = 0.0 + z_factor + roll_factor + pitch_factor - yaw_factor; //printf("r=%f p=%f zf=%f rf=%f pf=%f yf=%f a=%f b=%f c=%f d=%f\n", measured_state->roll, measured_state->pitch, z_factor, roll_factor, pitch_factor, yaw_factor, rotor_speeds->a, rotor_speeds->b, rotor_speeds->c, rotor_speeds->d); diff --git a/src/settings.h b/src/settings.h index cd078d3..a8f19a0 100644 --- a/src/settings.h +++ b/src/settings.h @@ -31,7 +31,7 @@ // Roll-factor PID parameters. #define PID_GAIN_ROLL_P 0.1 #define PID_GAIN_ROLL_I 0.0 -#define PID_GAIN_ROLL_D 0.0 +#define PID_GAIN_ROLL_D 0.02 #define MIN_INTEGRAL_ROLL -1.0 #define MAX_INTEGRAL_ROLL 1.0 @@ -41,8 +41,8 @@ // Fixed axis // Iteration 3: 4.82/545.7/0.0003971 #define PID_GAIN_PITCH_P 0.1 -#define PID_GAIN_PITCH_I 0.0 -#define PID_GAIN_PITCH_D 0.0 +#define PID_GAIN_PITCH_I 0.002 +#define PID_GAIN_PITCH_D 0.015 #define MIN_INTEGRAL_PITCH -1.0 #define MAX_INTEGRAL_PITCH 1.0