stuff

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);

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