Enabled closed loop feedback from gyro to outputs.HEADmaster

1 files changed, 133 insertions, 11 deletions

diff --git a/main.cpp b/main.cpp
index aa1df03..aaa3661 100644
--- a/main.cpp
+++ b/main.cpp
@@ -38,6 +38,44 @@ static Pin& cs_gyro = PB5;
 static Pin& esc_power = PA1;
 static Pin pwm_outputs[] = {PC6, PC7, PC8, PC9};
 
+volatile float g_throttle = 0;
+
+class PID {
+		float i_acc;
+		float last_error;
+		
+		const float dt = (1.0 / 400);
+	
+	public:
+		float Kp;
+		float Ki;
+		float Kd;
+		
+		PID() : i_acc(0), last_error(0), Kp(0), Ki(0), Kd(0) {}
+		
+		float update(float error) {
+			float corr = Kp * error + (i_acc += Ki * error * dt) + Kd * (error - last_error) / dt;
+			
+			last_error = error;
+			
+			return corr;
+		}
+		
+		void reset() {
+			i_acc = 0;
+			last_error = 0;
+		}
+};
+
+PID pid_x;
+PID pid_y;
+PID pid_z;
+
+volatile uint32_t gyro_calib = 0;
+float gyro_calib_x = 0;
+float gyro_calib_y = 0;
+float gyro_calib_z = 0;
+
 auto dev_desc = device_desc(0x200, 0, 0, 0, 64, 0x1234, 0x5678, 0, 0, 0, 0, 1);
 auto conf_desc = configuration_desc(3, 1, 0, 0xc0, 0,
 	interface_desc(0, 0, 3, 0xff, 0x00, 0x00, 0,
@@ -165,7 +203,43 @@ class USB_TM : public USB_class_driver {
 					
 					switch(bufp[0]) {
 						case 2:
-							TIM8.CCR1 = TIM8.CCR2 = TIM8.CCR3 = TIM8.CCR4 = 1000 + bufp[1] * 2;
+							g_throttle = float(bufp[1]) / 128.0f * 0.7;
+							break;
+						
+						case 3:
+							pid_x.Kp = float(bufp[1]) / 128.0f * 0.4;
+							pid_y.Kp = float(bufp[1]) / 128.0f * 0.4;
+							break;
+						
+						case 4:
+							pid_x.Ki = float(bufp[1]) / 128.0f * 0.4;
+							pid_y.Ki = float(bufp[1]) / 128.0f * 0.4;
+							break;
+						
+						case 5:
+							pid_x.Kd = float(bufp[1]) / 128.0f * 0.4;
+							pid_y.Kd = float(bufp[1]) / 128.0f * 0.4;
+							break;
+						
+						case 6:
+							pid_z.Kp = float(bufp[1]) / 128.0f * 0.4;
+							break;
+						
+						case 7:
+							pid_z.Kp = float(bufp[1]) / 128.0f * 0.4;
+							break;
+						
+						case 8:
+							pid_z.Kp = float(bufp[1]) / 128.0f * 0.4;
+							break;
+						
+						case 23:
+							if(bufp[1]) {
+								gyro_calib = 256;
+								gyro_calib_x = 0;
+								gyro_calib_y = 0;
+								gyro_calib_z = 0;
+							}
 							break;
 					}
 				}
@@ -279,24 +353,70 @@ int main() {
 	TIM8.CCR4 = 1000;
 	TIM8.CR1 = 0x05;
 	
+	float abst = 0;
+	
 	while(1) {
 		led_error.toggle();
-		Time::sleep(10);
+		
+		// Wait for new period.
+		while(!(TIM8.SR & 1)) {
+			Thread::yield();
+		}
+		
+		TIM8.SR = 0;
+		
+		float throttle = g_throttle;
 		
 		gyro.update();
-		accel.update();
-		magn.update();
+		//accel.update();
+		//magn.update();
+		
+		if(gyro_calib) {
+			gyro_calib--;
+			gyro_calib_x += gyro.x / 256;
+			gyro_calib_y += gyro.y / 256;
+			gyro_calib_z += gyro.z / 256;
+		}
+		
+		gyro.x -= gyro_calib_x;
+		gyro.y -= gyro_calib_y;
+		gyro.z -= gyro_calib_z;
+		
+		float stab_x = pid_x.update(-gyro.x);
+		float stab_y = pid_y.update(-gyro.y);
+		float stab_z = pid_z.update(-gyro.z);
+		
+		if(throttle < 0.05) {
+			stab_x = 0;
+			stab_y = 0;
+			stab_z = 0;
+			pid_x.reset();
+			pid_y.reset();
+			pid_z.reset();
+		}
 		
-		int16_t buf[] = {
+		float motors[] = {
+			throttle - stab_x - stab_y + stab_z,
+			throttle - stab_x + stab_y - stab_z,
+			throttle + stab_x + stab_y + stab_z,
+			throttle + stab_x - stab_y - stab_z
+		};
+		
+		TIM8.CCR1 = 1000 + motors[0] * 1000;
+		TIM8.CCR2 = 1000 + motors[1] * 1000;
+		TIM8.CCR3 = 1000 + motors[2] * 1000;
+		TIM8.CCR4 = 1000 + motors[3] * 1000;
+		
+		float buf[] = {
+			abst,
 			gyro.x,
 			gyro.y,
 			gyro.z,
-			accel.x,
-			accel.y,
-			accel.z,
-			magn.x,
-			magn.y,
-			magn.z,
+			stab_x,
+			stab_y,
+			stab_z,
+			throttle,
+			0,
 		};
 		
 		if(usb.ep_ready(2)) {
@@ -304,5 +424,7 @@ int main() {
 		} else {
 			usb_rblog.log("Busy.");
 		}
+		
+		abst += 1.0 / 400.0;
 	}
 }