stuff

Seine2017 · Mar 13, 2017 · 16bc2d7 · 16bc2d7
1 parent a34e65e
commit 16bc2d7
Show file tree

Hide file tree

Showing 3 changed files with 7 additions and 7 deletions.
diff --git a/src/control.c b/src/control.c
@@ -79,11 +79,11 @@ void control_cycle(measured_state_t *measured_state,
 
   //printf("z_vel error=%f-%f zfactor=%f\n", desired_state->z_vel, measured_state->z_vel, z_factor);
 
-  printf("r=%f p=%f zf=%f rf=%f pf=%f yf=%f\n", measured_state->roll, measured_state->pitch, z_factor, roll_factor, pitch_factor, yaw_factor);
-
   // 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->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/escs.c b/src/escs.c
@@ -134,5 +134,5 @@ void escs_update(rotor_speeds_t *rotor_speeds) {
   escs_levels.c = level_c;
   escs_levels.d = level_d;
 
-  //printf("a: raw=%f, filtered=%f, level=%d\n", rotor_speeds->a, filtered_a, level_a);
+  printf("%d,%d,%d,%d\n", level_a, level_b, level_c, level_d);
 }
diff --git a/src/imu.c b/src/imu.c
@@ -454,14 +454,14 @@ void imu_read(measured_state_t *destination){
 	}
 	prev_time = curr_time;
 
-	//printf("gx=%f gy=%f ax=%f ay=%f az=%f\n", gyro_x, gyro_y, accel_x, accel_y, accel_z);
+	MadgwickAHRSupdateIMU(gyro_x, gyro_y, gyro_z, accel_x, -accel_y, accel_z, dt);
 
-	MadgwickAHRSupdateIMU(gyro_x, gyro_y, gyro_z, accel_x, accel_y, accel_z, dt);
-
-	destination->roll = -atan2f(2*(q0*q1+q2*q3), 1-2*(q1*q1+q2*q2));
+	destination->roll = atan2f(2*(q0*q1+q2*q3), 1-2*(q1*q1+q2*q2));
 	destination->pitch = asinf(2*(q0*q2-q1*q3));
 	destination->yaw_vel = gyro_z;
 
+	//printf("gx=%f gy=%f ax=%f ay=%f az=%f r=%f p=%f\n", gyro_x, gyro_y, accel_x, accel_y, accel_z, destination->roll, destination->pitch);
+
 	static float z_vel = 0.0;
 	float z_accel = get_vert_accel(accel_x, accel_y, accel_z);
 	z_vel += dt * z_accel * 50.0;