dos2unix

1 files changed, 253 insertions, 253 deletions

diff --git a/main.cpp b/main.cpp
index 3bebf74..4d3240c 100644
--- a/main.cpp
+++ b/main.cpp
@@ -1,253 +1,253 @@
-#include "thread.h"

-#include "usbserial.h"

-#include "itg3200.h"

-#include "bma150.h"

-#include "ak8975.h"

-#include "ppmsum.h"

-#include "motormixer.h"

-

-#include <ch.h>

-#include <hal.h>

-

-#include "IMU.h"

-

-class LEDThread : public BaseThread<LEDThread, 128> {

-	public:

-		noreturn_t thread_main() {

-			systime_t time = chTimeNow();     // T0

-			while (TRUE) {

-				time += MS2ST(1000);            // Next deadline

-				palClearPad(GPIOA, 5);

-				chThdSleepUntil(time);

-				time += MS2ST(1000);            // Next deadline

-				palSetPad(GPIOA, 5);

-				chThdSleepUntil(time);

-			}

-		}

-};

-

-LEDThread led_thread;

-

-PPMSum ppmsum;

-MotorMixer motors;

-

-class USBThread : public BaseThread<USBThread, 256> {

-	private:

-		typedef enum {W_S, W_N, W_V} w_s_t;

-	

-	public:

-		USBSerial* usbs;

-		

-		uint8_t data[9];

-		

-		noreturn_t thread_main() {

-			for(int i = 0; i < 9; i++) {

-				data[i] = 0;

-			}

-			

-			w_s_t w_s = W_S;

-			uint8_t w_n = 0;

-			

-			while(1) {

-				size_t buffer = usbs->getc();

-				if(buffer >= 0 && buffer < 256) {

-					if(w_s == W_S && buffer == 'S') {

-						w_s = W_N;

-					} else if(w_s == W_N && buffer >= '1' && buffer <= '9') {

-						w_s = W_V;

-						w_n = buffer - '1';

-					} else if(w_s == W_V) {

-						w_s = W_S;

-						data[w_n] = buffer;

-					} else {

-						w_s = W_S;

-					}

-				}

-			}

-		}

-};

-

-USBThread usb_thread;

-USBSerial usbs;

-

-#include "foo.h"

-#include <cmath>

-

-uint8_t syncword[] = {0xff, 0x00, 0xaa, 0x55};

-uint8_t buf[64];

-int16_t* sensordata = (int16_t*)buf;

-

-template<class T>

-inline void saturate(T& var, int absmax) {

-	if(var > absmax) {

-		var = absmax;

-	} else if(var < -absmax) {

-		var = -absmax;

-	}

-}

-

-class I2CThread : public BaseThread<I2CThread, 256> {

-	public:

-		ITG3200 gyro;

-		BMA150 acc;

-		AK8975 magn;

-		

-		int16_t x, y, z;

-		

-		noreturn_t thread_main() {

-			I2CSensor::enable_bus();

-			

-			gyro.init();

-			acc.init();

-			magn.init();

-			

-			systime_t time = chTimeNow();

-			

-			int32_t pitch_angle_accum = 0;

-			int32_t roll_angle_accum = 0;

-			

-			while (1) {

-				gyro.update();

-				acc.update();

-				magn.update();

-				x = gyro.x;

-				y = gyro.y;

-				z = gyro.z;

-				

-				IMUupdate(gyro.x * 0.0012141420883438813, gyro.y * 0.0012141420883438813, gyro.z * 0.0012141420883438813, acc.x, acc.y, acc.z);

-				

-				//float pitch = asinf(2*(q0*q2 - q3*q1));

-				//int16_t pitch = atan2f(2*(q2*q3 + q0*q1), 1 - 2 * (q1*q1 + q2*q2)) / M_PI * 32767;

-				//int16_t roll = atan2f(2*(-q1*q3 + q0*q2), 1 - 2 * (q1*q1 + q2*q2)) / M_PI * 32767;

-				//int16_t yaw = atan2f(2*(q2*q1 + q0*q3), 1 - 2 * (q3*q3 + q2*q2)) / M_PI * 32767;

-				

-				float norm_x = 2*(q0*q2 - q1*q3);

-				float norm_y = 2*(q0*q1 + q2*q3);

-				float norm_z = (1 - 2*(q1*q1 + q2*q2));

-				

-				float elev = acosf(norm_z);

-				float azim = atan2f(norm_y, norm_x);

-				

-				int16_t pitch = elev * sinf(azim) / M_PI * 32767;

-				int16_t roll = elev * cosf(azim) / M_PI * 32767;

-				int16_t yaw = 0;

-				

-				sensordata[0] = gyro.x;

-				sensordata[1] = gyro.y;

-				sensordata[2] = gyro.z;

-				sensordata[3] = acc.x;

-				sensordata[4] = acc.y;

-				sensordata[5] = acc.z;

-				sensordata[6] = magn.x;

-				sensordata[7] = magn.y;

-				sensordata[8] = magn.z;

-				sensordata[9] = pitch;

-				sensordata[10] = roll;

-				sensordata[11] = yaw;

-				

-				usbs.write(syncword, sizeof(syncword));

-				usbs.write(buf, sizeof(buf));

-				

-				/*usbprintf(usbs, "%6d, %6d, %6d | %6d, %6d, %6d | %6d, %6d, %6d | %6d, %6d, %6d, %6d | %6d, %6d, %6d\r\n",

-					gyro.x, gyro.y, gyro.z,

-					acc.x, acc.y, acc.z,

-					magn.x, magn.y, magn.z,

-					int(q0 * 10000), int(q1 * 10000), int(q2 * 10000), int(q3 * 10000),

-					int(pitch * 10000), int(roll * 10000), int(yaw * 10000));*/

-				

-				int16_t pitch_angle_target = (ppmsum.data[1] - 500) * 8;

-				int16_t roll_angle_target = (ppmsum.data[0] - 500) * 8;

-				

-				int16_t pitch_angle_error = pitch_angle_target - pitch;

-				int16_t roll_angle_error = roll_angle_target - roll;

-				

-				// 25 deg max error.

-				saturate(pitch_angle_error, 4551);

-				saturate(roll_angle_error, 4551);

-				

-				pitch_angle_accum += pitch_angle_error;

-				roll_angle_accum += roll_angle_error;

-				

-				// 20 deg s max error.

-				saturate(pitch_angle_error, 364088);

-				saturate(roll_angle_error, 364088);

-				

-				int32_t pitch_rate_target = (pitch_angle_error * 4 * 65536 + pitch_angle_accum * 98) >> 16;

-				int32_t roll_rate_target = (roll_angle_error * 4 * 65536 + roll_angle_accum * 98) >> 16;

-				

-				int16_t pitch_rate_comp = ((pitch_rate_target - (gyro.x * 4000 / 360)) * 6 * 36) >> 16;

-				int16_t roll_rate_comp = ((roll_rate_target - (gyro.y * 4000 / 360)) * 6 * 36) >> 16;

-				

-				saturate(pitch_rate_comp, 250);

-				saturate(roll_rate_comp, 250);

-				

-				motors.update(ppmsum.data[2], pitch_rate_comp, roll_rate_comp, 0);

-				

-				time += MS2ST(10);

-				if(time > chTimeNow()) {

-					chThdSleepUntil(time);

-				}

-			}

-		}

-};

-

-I2CThread i2c_thread;

-

-static const ADCConversionGroup adcgrpcfg = {

-	FALSE,

-	2,

-	0,

-	0,

-	0,

-	0,

-	0,

-	ADC_SQR1_NUM_CH(2),

-	0,

-	ADC_SQR3_SQ2_N(ADC_CHANNEL_IN14) | ADC_SQR3_SQ1_N(ADC_CHANNEL_IN15)

-};

-

-class ADCThread : public BaseThread<ADCThread, 128> {

-	private:

-		adcsample_t adc_samples[2];

-		

-	public:

-		noreturn_t thread_main() {

-			adcStart(&ADCD1, NULL);

-			

-			systime_t time = chTimeNow();

-			while (TRUE) {

-				adcStartConversion(&ADCD1, &adcgrpcfg, adc_samples, 1);

-				sensordata[12] = adc_samples[0] * 1265 / 1000;

-				sensordata[13] = adc_samples[1] * 2201 / 1000;

-				

-				time += MS2ST(1000);

-				chThdSleepUntil(time);

-			}

-		}

-};

-

-ADCThread adc_thread;

-

-int main(void) {

-	halInit();

-	chSysInit();

-	

-	led_thread.start();

-	

-	ppmsum.start();

-	

-	usbs.init();

-	

-	i2c_thread.start();

-	

-	adc_thread.start();

-	

-	motors.start();

-	

-	usb_thread.usbs = &usbs;

-	usb_thread.start();

-	

-	while (1) {

-		chThdSleepMilliseconds(1000);

-	}

-}

+#include "thread.h"
+#include "usbserial.h"
+#include "itg3200.h"
+#include "bma150.h"
+#include "ak8975.h"
+#include "ppmsum.h"
+#include "motormixer.h"
+
+#include <ch.h>
+#include <hal.h>
+
+#include "IMU.h"
+
+class LEDThread : public BaseThread<LEDThread, 128> {
+	public:
+		noreturn_t thread_main() {
+			systime_t time = chTimeNow();     // T0
+			while (TRUE) {
+				time += MS2ST(1000);            // Next deadline
+				palClearPad(GPIOA, 5);
+				chThdSleepUntil(time);
+				time += MS2ST(1000);            // Next deadline
+				palSetPad(GPIOA, 5);
+				chThdSleepUntil(time);
+			}
+		}
+};
+
+LEDThread led_thread;
+
+PPMSum ppmsum;
+MotorMixer motors;
+
+class USBThread : public BaseThread<USBThread, 256> {
+	private:
+		typedef enum {W_S, W_N, W_V} w_s_t;
+	
+	public:
+		USBSerial* usbs;
+		
+		uint8_t data[9];
+		
+		noreturn_t thread_main() {
+			for(int i = 0; i < 9; i++) {
+				data[i] = 0;
+			}
+			
+			w_s_t w_s = W_S;
+			uint8_t w_n = 0;
+			
+			while(1) {
+				size_t buffer = usbs->getc();
+				if(buffer >= 0 && buffer < 256) {
+					if(w_s == W_S && buffer == 'S') {
+						w_s = W_N;
+					} else if(w_s == W_N && buffer >= '1' && buffer <= '9') {
+						w_s = W_V;
+						w_n = buffer - '1';
+					} else if(w_s == W_V) {
+						w_s = W_S;
+						data[w_n] = buffer;
+					} else {
+						w_s = W_S;
+					}
+				}
+			}
+		}
+};
+
+USBThread usb_thread;
+USBSerial usbs;
+
+#include "foo.h"
+#include <cmath>
+
+uint8_t syncword[] = {0xff, 0x00, 0xaa, 0x55};
+uint8_t buf[64];
+int16_t* sensordata = (int16_t*)buf;
+
+template<class T>
+inline void saturate(T& var, int absmax) {
+	if(var > absmax) {
+		var = absmax;
+	} else if(var < -absmax) {
+		var = -absmax;
+	}
+}
+
+class I2CThread : public BaseThread<I2CThread, 256> {
+	public:
+		ITG3200 gyro;
+		BMA150 acc;
+		AK8975 magn;
+		
+		int16_t x, y, z;
+		
+		noreturn_t thread_main() {
+			I2CSensor::enable_bus();
+			
+			gyro.init();
+			acc.init();
+			magn.init();
+			
+			systime_t time = chTimeNow();
+			
+			int32_t pitch_angle_accum = 0;
+			int32_t roll_angle_accum = 0;
+			
+			while (1) {
+				gyro.update();
+				acc.update();
+				magn.update();
+				x = gyro.x;
+				y = gyro.y;
+				z = gyro.z;
+				
+				IMUupdate(gyro.x * 0.0012141420883438813, gyro.y * 0.0012141420883438813, gyro.z * 0.0012141420883438813, acc.x, acc.y, acc.z);
+				
+				//float pitch = asinf(2*(q0*q2 - q3*q1));
+				//int16_t pitch = atan2f(2*(q2*q3 + q0*q1), 1 - 2 * (q1*q1 + q2*q2)) / M_PI * 32767;
+				//int16_t roll = atan2f(2*(-q1*q3 + q0*q2), 1 - 2 * (q1*q1 + q2*q2)) / M_PI * 32767;
+				//int16_t yaw = atan2f(2*(q2*q1 + q0*q3), 1 - 2 * (q3*q3 + q2*q2)) / M_PI * 32767;
+				
+				float norm_x = 2*(q0*q2 - q1*q3);
+				float norm_y = 2*(q0*q1 + q2*q3);
+				float norm_z = (1 - 2*(q1*q1 + q2*q2));
+				
+				float elev = acosf(norm_z);
+				float azim = atan2f(norm_y, norm_x);
+				
+				int16_t pitch = elev * sinf(azim) / M_PI * 32767;
+				int16_t roll = elev * cosf(azim) / M_PI * 32767;
+				int16_t yaw = 0;
+				
+				sensordata[0] = gyro.x;
+				sensordata[1] = gyro.y;
+				sensordata[2] = gyro.z;
+				sensordata[3] = acc.x;
+				sensordata[4] = acc.y;
+				sensordata[5] = acc.z;
+				sensordata[6] = magn.x;
+				sensordata[7] = magn.y;
+				sensordata[8] = magn.z;
+				sensordata[9] = pitch;
+				sensordata[10] = roll;
+				sensordata[11] = yaw;
+				
+				usbs.write(syncword, sizeof(syncword));
+				usbs.write(buf, sizeof(buf));
+				
+				/*usbprintf(usbs, "%6d, %6d, %6d | %6d, %6d, %6d | %6d, %6d, %6d | %6d, %6d, %6d, %6d | %6d, %6d, %6d\r\n",
+					gyro.x, gyro.y, gyro.z,
+					acc.x, acc.y, acc.z,
+					magn.x, magn.y, magn.z,
+					int(q0 * 10000), int(q1 * 10000), int(q2 * 10000), int(q3 * 10000),
+					int(pitch * 10000), int(roll * 10000), int(yaw * 10000));*/
+				
+				int16_t pitch_angle_target = (ppmsum.data[1] - 500) * 8;
+				int16_t roll_angle_target = (ppmsum.data[0] - 500) * 8;
+				
+				int16_t pitch_angle_error = pitch_angle_target - pitch;
+				int16_t roll_angle_error = roll_angle_target - roll;
+				
+				// 25 deg max error.
+				saturate(pitch_angle_error, 4551);
+				saturate(roll_angle_error, 4551);
+				
+				pitch_angle_accum += pitch_angle_error;
+				roll_angle_accum += roll_angle_error;
+				
+				// 20 deg s max error.
+				saturate(pitch_angle_error, 364088);
+				saturate(roll_angle_error, 364088);
+				
+				int32_t pitch_rate_target = (pitch_angle_error * 4 * 65536 + pitch_angle_accum * 98) >> 16;
+				int32_t roll_rate_target = (roll_angle_error * 4 * 65536 + roll_angle_accum * 98) >> 16;
+				
+				int16_t pitch_rate_comp = ((pitch_rate_target - (gyro.x * 4000 / 360)) * 6 * 36) >> 16;
+				int16_t roll_rate_comp = ((roll_rate_target - (gyro.y * 4000 / 360)) * 6 * 36) >> 16;
+				
+				saturate(pitch_rate_comp, 250);
+				saturate(roll_rate_comp, 250);
+				
+				motors.update(ppmsum.data[2], pitch_rate_comp, roll_rate_comp, 0);
+				
+				time += MS2ST(10);
+				if(time > chTimeNow()) {
+					chThdSleepUntil(time);
+				}
+			}
+		}
+};
+
+I2CThread i2c_thread;
+
+static const ADCConversionGroup adcgrpcfg = {
+	FALSE,
+	2,
+	0,
+	0,
+	0,
+	0,
+	0,
+	ADC_SQR1_NUM_CH(2),
+	0,
+	ADC_SQR3_SQ2_N(ADC_CHANNEL_IN14) | ADC_SQR3_SQ1_N(ADC_CHANNEL_IN15)
+};
+
+class ADCThread : public BaseThread<ADCThread, 128> {
+	private:
+		adcsample_t adc_samples[2];
+		
+	public:
+		noreturn_t thread_main() {
+			adcStart(&ADCD1, NULL);
+			
+			systime_t time = chTimeNow();
+			while (TRUE) {
+				adcStartConversion(&ADCD1, &adcgrpcfg, adc_samples, 1);
+				sensordata[12] = adc_samples[0] * 1265 / 1000;
+				sensordata[13] = adc_samples[1] * 2201 / 1000;
+				
+				time += MS2ST(1000);
+				chThdSleepUntil(time);
+			}
+		}
+};
+
+ADCThread adc_thread;
+
+int main(void) {
+	halInit();
+	chSysInit();
+	
+	led_thread.start();
+	
+	ppmsum.start();
+	
+	usbs.init();
+	
+	i2c_thread.start();
+	
+	adc_thread.start();
+	
+	motors.start();
+	
+	usb_thread.usbs = &usbs;
+	usb_thread.start();
+	
+	while (1) {
+		chThdSleepMilliseconds(1000);
+	}
+}