Disabled unported code.

2 files changed, 61 insertions, 42 deletions

diff --git a/main.cpp b/main.cpp
index 309ff60..47e7c51 100644
--- a/main.cpp
+++ b/main.cpp
@@ -65,15 +65,15 @@ AHRS ahrs(I2C1);
 
 volatile uint16_t motors[4];
 
-GPS gps;
+//GPS gps;
 
 void gps_thread_main() {
 	while(1) {
-		P<GPSMsg> msg = gps.read();
+		//P<GPSMsg> msg = gps.read();
 		
-		if(msg->n < 128) {
-			xbee_send(2, msg->n, msg->buf);
-		}
+		//if(msg->n < 128) {
+		//	xbee_send(2, msg->n, msg->buf);
+		//}
 	}
 }
 
@@ -100,9 +100,9 @@ int main() {
 	led_red.set_mode(Pin::Output);
 	led_blue.set_mode(Pin::Output);
 	
+	//RCC.enable(RCC.DMA1);
+	//RCC.enable(RCC.ADC1);
 	
-	RCC.enable(RCC.DMA1);
-	RCC.enable(RCC.ADC1);
 	PB6.set_af(7);
 	PB7.set_af(7);
 	PB6.set_mode(Pin::Output);
@@ -114,58 +114,76 @@ int main() {
 	I2C1.enable(PB8, PB9);
 	ahrs.init();
 	
-	PPMSum ppmsum;
-	ppmsum.enable();
+	//PPMSum ppmsum;
+	//ppmsum.enable();
+	
+	//RCC.enable(RCC.TIM2);
+	//TIM2.PSC = 72;
+	//TIM2.ARR = 5000;
+	//TIM2.CCER = 0x1111;
+	//TIM2.CCMR1 = 0x6868;
+	//TIM2.CCMR2 = 0x6868;
 	
-	RCC.enable(RCC.TIM2);
-	TIM2.PSC = 72;
-	TIM2.ARR = 5000;
-	TIM2.CCER = 0x1111;
-	TIM2.CCMR1 = 0x6868;
-	TIM2.CCMR2 = 0x6868;
+	//TIM2.CR1 = 0x05;
 	
-	TIM2.CR1 = 0x05;
+	//PID pid_pitch(6000, 0, 0);
+	//PID pid_roll(6000, 0, 0);
+	//PID pid_yaw(6000, 0, 0);
 	
-	PID pid_pitch(6000, 0, 0);
-	PID pid_roll(6000, 0, 0);
-	PID pid_yaw(6000, 0, 0);
+	RCC.enable(RCC.USART1);
 	
 	usart_enable();
-	gps.enable();
+	//gps.enable();
 	
 	telemetry_thread.start();
 	//gps_thread.start();
 	
 	while(1) {
+		led_green.on();
+		Time::sleep(100);
+		led_green.off();
+		
+		led_yellow.on();
+		Time::sleep(100);
+		led_yellow.off();
+		
+		led_red.on();
+		Time::sleep(100);
+		led_red.off();
+		
+		led_blue.on();
+		Time::sleep(100);
+		led_blue.off();
+		
 		// Wait for a new update.
-		while(!(TIM2.SR & 0x01)) {
-			Thread::yield();
-		}
-		TIM2.SR = 0;
+		//while(!(TIM2.SR & 0x01)) {
+		//	Thread::yield();
+		//}
+		//TIM2.SR = 0;
 		
 		// Update AHRS.
 		ahrs.update();
 		
 		// Update filter.
-		int16_t throttle = ppmsum.channels[2] - 1000;
-		int16_t pitch = pid_pitch.update((ppmsum.channels[1] - 1500) * 1 - ahrs.gyro.x);
-		int16_t roll = pid_roll.update((ppmsum.channels[0] - 1500) * 1 - ahrs.gyro.y);
-		int16_t yaw = pid_yaw.update((ppmsum.channels[3] - 1500) * -1 - ahrs.gyro.z);
+		//int16_t throttle = ppmsum.channels[2] - 1000;
+		//int16_t pitch = pid_pitch.update((ppmsum.channels[1] - 1500) * 1 - ahrs.gyro.x);
+		//int16_t roll = pid_roll.update((ppmsum.channels[0] - 1500) * 1 - ahrs.gyro.y);
+		//int16_t yaw = pid_yaw.update((ppmsum.channels[3] - 1500) * -1 - ahrs.gyro.z);
 		
-		int16_t max = throttle > 250 ? 250 : throttle;
-		saturate(pitch, max);
-		saturate(roll, max);
-		saturate(yaw, max);
+		//int16_t max = throttle > 250 ? 250 : throttle;
+		//saturate(pitch, max);
+		//saturate(roll, max);
+		//saturate(yaw, max);
 		
 		// Generate motor mix.
-		motors[0] = limit(throttle + pitch - roll + yaw, 0, 1000);
-		motors[1] = limit(throttle - pitch - roll - yaw, 0, 1000);
-		motors[2] = limit(throttle - pitch + roll + yaw, 0, 1000);
-		motors[3] = limit(throttle + pitch + roll - yaw, 0, 1000);
+		//motors[0] = limit(throttle + pitch - roll + yaw, 0, 1000);
+		//motors[1] = limit(throttle - pitch - roll - yaw, 0, 1000);
+		//motors[2] = limit(throttle - pitch + roll + yaw, 0, 1000);
+		//motors[3] = limit(throttle + pitch + roll - yaw, 0, 1000);
 		
-		TIM2.CCR1 = 1000 + motors[0];
-		TIM2.CCR2 = 1000 + motors[1];
-		TIM2.CCR3 = 1000 + motors[2];
-		TIM2.CCR4 = 1000 + motors[3];
+		//TIM2.CCR1 = 1000 + motors[0];
+		//TIM2.CCR2 = 1000 + motors[1];
+		//TIM2.CCR3 = 1000 + motors[2];
+		//TIM2.CCR4 = 1000 + motors[3];
 	}
 }
diff --git a/telemetry.cpp b/telemetry.cpp
index d44412f..781fb60 100644
--- a/telemetry.cpp
+++ b/telemetry.cpp
@@ -12,6 +12,7 @@ extern AHRS ahrs;
 volatile uint16_t dmabuf[2];
 
 void telemetry_main() {
+	/*
 	ADC1.CR2 = 0x9;
 	while(ADC1.CR2 & 0x8);
 	ADC1.CR2 = 0x5;
@@ -28,7 +29,7 @@ void telemetry_main() {
 	ADC1.CR1 = 0x100;
 	ADC1.CR2 = 0x103;
 	ADC1.CR2 = 0x103;
-	
+	*/
 	while(1) {
 		uint16_t buf[] = {
 			ahrs.gyro.x,
@@ -52,4 +53,4 @@ void telemetry_main() {
 
 uint32_t telemetry_stack[1024];
 
-Thread telemetry_thread(telemetry_stack, sizeof(telemetry_stack), telemetry_main);
\ No newline at end of file
+Thread telemetry_thread(telemetry_stack, sizeof(telemetry_stack), telemetry_main);