working rc->mixer->blctrls path

include/at91_adc.h

@@ -11,6 +11,7 @@
 #define ADC_ACC_YAW	5
 #define ADC_VOLTAGE	6
 
+#define ADC_CAL_NONE	0
 #define ADC_CAL_GYRO	1
 #define ADC_CAL_ACC	2
 

include/at91_tc1.h

@@ -11,6 +11,8 @@ struct rc_values {
 };
 
 uint32_t rcontrol_getvalues(struct rc_values *rc);
+uint32_t rcontrol_getswitches(struct rc_values *rc);
+
 void rcontrol_calibrate(uint32_t mode);
 void rcontrol_print_cal(void);
 

main.c

@@ -33,54 +33,90 @@
 #include <stdio.h>
 #include <string.h>
 
-static uint32_t pitc_test(struct pitc_timer *timer)
+static uint32_t global_state;
+
+#define MOTORS_RUNNING		0x0001
+#define STICK_CALIBRATION	0x0002
+
+static uint32_t stick_timer_cb(struct pitc_timer *timer)
 {
 	static uint32_t i;
 	*AT91C_PIOA_SODR = i;
 	i = i ^ LED_GREEN;
 	*AT91C_PIOA_CODR = i;
-/*
+
 	struct rc_values rc;
-	uint32_t count = rcontrol_getvalues(&rc);
+	uint32_t count = rcontrol_getswitches(&rc);
 
-	printf("%ld channels: ", count);
-	uint32_t j;
-	for (j = 0; j < count; j++)
-		printf("%+5d ", rc.chan[j]);
+	static uint32_t stick_cal_count;
 
-	printf("\r");
+	if (global_state & MOTORS_RUNNING) {
+		if ((rc.chan[2] < 0 && rc.chan[3] > 0) || count < 4)
+			global_state &= ~MOTORS_RUNNING;
 
-	static uint32_t cnt;
+	} else if (global_state & STICK_CALIBRATION) {
+		stick_cal_count--;
+		if (stick_cal_count == 0) {
+			rcontrol_calibrate(RC_CAL_END);
+			rcontrol_print_cal();
+			global_state &= ~STICK_CALIBRATION;
+		}
 
-	if (cnt++ == 300) {
-		rcontrol_calibrate(RC_CAL_END);
-		printf("\n\r");
-		rcontrol_print_cal();
-	}
-*/
+	} else {
+		if (rc.chan[2] < 0 && rc.chan[3] < 0)
+			global_state |= MOTORS_RUNNING;
 
-	adc_trigger();
+		if (rc.chan[2] > 0 && rc.chan[3] > 0)
+			adc_calibrate(ADC_CAL_GYRO);
 
-	return PITC_RESTART_TIMER;
-}
+		if (rc.chan[2] > 0 && rc.chan[3] < 0) {
+			adc_calibrate(ADC_CAL_ACC);
 
-static struct pitc_timer pitc_test_timer = {
-	.interval = 100,
-	.func = &pitc_test,
-};
+			stick_cal_count = 3000;
+			global_state |= STICK_CALIBRATION;
+			rcontrol_calibrate(RC_CAL_START);
+		}
+	}
 
-static void motor_mixer(int16_t pitch, int16_t nick, int16_t roll, int16_t yaw)
-{
-	static uint8_t pwm[4];
+	rcontrol_getvalues(&rc);
 
-	pwm[0] = pitch + nick + yaw;
-	pwm[1] = pitch - nick + yaw;
-	pwm[2] = pitch + roll - yaw;
-	pwm[3] = pitch - roll - yaw;
+	static uint8_t pwm[4];
+	if (global_state & MOTORS_RUNNING) {
+		int32_t pitch = rc.chan[2] * 256;
+		int32_t gier = rc.chan[3] * 64;
+		int32_t nick = rc.chan[0] * 64;
+		int32_t roll = rc.chan[1] * 64;
+
+		int32_t data[4];
+		data[0] = (pitch - nick + gier) / 1024;
+		data[1] = (pitch + nick + gier) / 1024;
+		data[2] = (pitch + roll - gier) / 1024;
+		data[3] = (pitch - roll - gier) / 1024;
+
+		pwm[0] = (data[0] > 0x0f) ? data[0] : 0x0f;
+		pwm[1] = (data[1] > 0x0f) ? data[1] : 0x0f;
+		pwm[2] = (data[2] > 0x0f) ? data[2] : 0x0f;
+		pwm[3] = (data[3] > 0x0f) ? data[3] : 0x0f;
+
+	} else {
+		pwm[0] = 0x00;
+		pwm[1] = 0x00;
+		pwm[2] = 0x00;
+		pwm[3] = 0x00;
+	}
 
+//	printf("%3d %3d %3d %3d\n\r", pwm[0], pwm[1], pwm[2], pwm[3]);
 	twi_setpwm(pwm);
+
+//	adc_trigger();
+	return PITC_RESTART_TIMER;
 }
 
+static struct pitc_timer stick_timer = {
+	.interval = 10,
+	.func = &stick_timer_cb,
+};
+
 int main(void)
 {
 	/* LED outputs */
@@ -113,8 +149,7 @@ int main(void)
 	/* adc, need timer */
 	at91_adc_init();
 
-	pitc_schedule_timer(&pitc_test_timer);
+	pitc_schedule_timer(&stick_timer);
 
-//	rcontrol_calibrate(RC_CAL_START);
 	while (1);
 }

src/at91_adc.c

@@ -77,8 +77,33 @@ static uint32_t adc_calibrate_cb(struct pitc_timer *timer)
 		adc_cal_data[1] /= ADC_CAL_COUNT;
 		adc_cal_data[2] /= ADC_CAL_COUNT;
 
-		adc_cal_mode = 0;
+		if (adc_cal_mode == ADC_CAL_GYRO) {
+			adc_offset[ADC_GYRO_ROLL] = adc_cal_data[0];
+			adc_offset[ADC_GYRO_NICK] = adc_cal_data[1];
+			adc_offset[ADC_GYRO_YAW] = adc_cal_data[2];
+
+			// TODO: check for invalid values (not centered) -> return value?
+
+			printf("GYRO offsets: %4d/%4d/%4d\n\r",
+				adc_offset[ADC_GYRO_ROLL],
+				adc_offset[ADC_GYRO_NICK],
+				adc_offset[ADC_GYRO_YAW]);
+
+		} else {
+			adc_offset[ADC_ACC_ROLL] = adc_cal_data[0];
+			adc_offset[ADC_ACC_NICK] = adc_cal_data[1];
 
+			// TODO: only 1/2 offset?
+			adc_offset[ADC_ACC_YAW] = adc_cal_data[2];
+
+			// TODO: save to eeprom
+			printf("ACC  offsets: %4d/%4d/%4d\n\r",
+				adc_offset[ADC_ACC_ROLL],
+				adc_offset[ADC_ACC_NICK],
+				adc_offset[ADC_ACC_YAW]);
+		}
+
+		adc_cal_mode = ADC_CAL_NONE;
 		return PITC_REMOVE_TIMER;
 	}
 
@@ -103,43 +128,16 @@ void adc_trigger(void)
 
 void adc_calibrate(uint32_t mode)
 {
-	if (mode == ADC_CAL_GYRO || mode == ADC_CAL_ACC) {
+	if (adc_cal_mode != ADC_CAL_NONE)
+		return;
 
+	if (mode == ADC_CAL_GYRO || mode == ADC_CAL_ACC) {
 		adc_cal_count = ADC_CAL_COUNT;
 		adc_cal_mode = mode;
 		adc_cal_data[0] = 0;
 		adc_cal_data[1] = 0;
 		adc_cal_data[2] = 0;
 		pitc_schedule_timer(&adc_cal_timer);
-
-		// wait (1024 * 1ms ~> 1s)
-		while (adc_cal_mode);
-
-		if (mode == ADC_CAL_GYRO) {
-			adc_offset[ADC_GYRO_ROLL] = adc_cal_data[0];
-			adc_offset[ADC_GYRO_NICK] = adc_cal_data[1];
-			adc_offset[ADC_GYRO_YAW] = adc_cal_data[2];
-
-			// TODO: check for invalid values (not centered) -> return value?
-
-			printf("GYRO offsets: %4d/%4d/%4d\n\r",
-				adc_offset[ADC_GYRO_ROLL],
-				adc_offset[ADC_GYRO_NICK],
-				adc_offset[ADC_GYRO_YAW]);
-
-		} else {
-			adc_offset[ADC_ACC_ROLL] = adc_cal_data[0];
-			adc_offset[ADC_ACC_NICK] = adc_cal_data[1];
-
-			// TODO: only 1/2 offset?
-			adc_offset[ADC_ACC_YAW] = adc_cal_data[2];
-
-			// TODO: save to eeprom
-			printf("ACC  offsets: %4d/%4d/%4d\n\r",
-				adc_offset[ADC_ACC_ROLL],
-				adc_offset[ADC_ACC_NICK],
-				adc_offset[ADC_ACC_YAW]);
-		}
 	}
 }
 
@@ -183,8 +181,4 @@ void at91_adc_init(void)
 	aic->AIC_SMR[AT91C_ID_ADC] = IRQPRIO_ADC | AT91C_AIC_SRCTYPE_INT_HIGH_LEVEL;
 	aic->AIC_SVR[AT91C_ID_ADC] = (uint32_t)at91_adc_isr;
 	aic->AIC_IECR = (1<<AT91C_ID_ADC);
-
-	// TODO: trigger via rc
-	adc_calibrate(ADC_CAL_GYRO);
-	adc_calibrate(ADC_CAL_ACC);
 }

src/at91_tc1.c

@@ -27,7 +27,9 @@
 #define PULSE_MIN	0x0500
 #define PULSE_MAX	0x0D00
 #define PULSE_TIMEOUT	0x0F00
+
 #define PULSE_CENTER	0x08C0
+#define PULSE_SWITCH	0x01F0
 
 /* moving average filters */
 #define PULSE_FILTER_FAST	(1<<2)
@@ -132,11 +134,32 @@ uint32_t rcontrol_getvalues(struct rc_values *rc)
 		if (tmp < -VALUE_RANGE)
 			tmp = -VALUE_RANGE;
 
+		// TODO: stick mapping from eeprom
 		rc->chan[i] = tmp;
 	}
 	return cnt;
 }
 
+uint32_t rcontrol_getswitches(struct rc_values *rc)
+{
+	if (valid < 5)
+		return 0;
+
+	uint32_t i;
+	uint32_t cnt = MIN(ROUND_DIV256(count), ARRAY_SIZE(ch_data));
+	for (i = 0; i < cnt; i++) {
+		if (ch_data[i].width > (PULSE_CENTER + PULSE_SWITCH))
+			rc->chan[i] = VALUE_RANGE;
+
+		else if (ch_data[i].width < (PULSE_CENTER - PULSE_SWITCH))
+			rc->chan[i] = -VALUE_RANGE;
+
+		else
+			rc->chan[i] = 0;
+	}
+	return cnt;
+}
+
 void rcontrol_calibrate(uint32_t mode)
 {
 	uint32_t i;