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);
+	static uint32_t stick_cal_count;
-	uint32_t j;
-	for (j = 0; j < count; j++)
-		printf("%+5d ", rc.chan[j]);
 
-	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 (cnt++ == 300) {
+		if (stick_cal_count == 0) {
 			rcontrol_calibrate(RC_CAL_END);
-		printf("\n\r");
 			rcontrol_print_cal();
+			global_state &= ~STICK_CALIBRATION;
 		}
-*/
 
-	adc_trigger();
+	} else {
+		if (rc.chan[2] < 0 && rc.chan[3] < 0)
+			global_state |= MOTORS_RUNNING;
 
+		if (rc.chan[2] > 0 && rc.chan[3] > 0)
+			adc_calibrate(ADC_CAL_GYRO);
+
+		if (rc.chan[2] > 0 && rc.chan[3] < 0) {
+			adc_calibrate(ADC_CAL_ACC);
+
+			stick_cal_count = 3000;
+			global_state |= STICK_CALIBRATION;
+			rcontrol_calibrate(RC_CAL_START);
+		}
+	}
+
+	rcontrol_getvalues(&rc);
+
+	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 pitc_test_timer = {
+static struct pitc_timer stick_timer = {
-	.interval = 100,
+	.interval = 10,
-	.func = &pitc_test,
+	.func = &stick_timer_cb,
 };
 
-static void motor_mixer(int16_t pitch, int16_t nick, int16_t roll, int16_t yaw)
-{
-	static uint8_t pwm[4];
-
-	pwm[0] = pitch + nick + yaw;
-	pwm[1] = pitch - nick + yaw;
-	pwm[2] = pitch + roll - yaw;
-	pwm[3] = pitch - roll - yaw;
-
-	twi_setpwm(pwm);
-}
-
 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,45 +77,7 @@ 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) {
-
-		return PITC_REMOVE_TIMER;
-	}
-
-	return PITC_RESTART_TIMER;
-}
-
-static struct pitc_timer adc_cal_timer = {
-	.interval = 1,
-	.func = &adc_calibrate_cb,
-};
-
-void adc_trigger(void)
-{
-	printf("R:%4d:%4d N:%4d:%4d Y:%4d:%4d U:%4d\n\r",
-		adc_result[ADC_GYRO_ROLL], adc_result[ADC_ACC_ROLL],
-		adc_result[ADC_GYRO_NICK], adc_result[ADC_ACC_NICK],
-		adc_result[ADC_GYRO_YAW], adc_result[ADC_ACC_YAW],
-		adc_result[ADC_VOLTAGE]);
-
-	*AT91C_ADC_CR = AT91C_ADC_START;
-}
-
-void adc_calibrate(uint32_t mode)
-{
-	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];
@@ -140,6 +102,42 @@ void adc_calibrate(uint32_t mode)
 				adc_offset[ADC_ACC_NICK],
 				adc_offset[ADC_ACC_YAW]);
 		}
+
+		adc_cal_mode = ADC_CAL_NONE;
+		return PITC_REMOVE_TIMER;
+	}
+
+	return PITC_RESTART_TIMER;
+}
+
+static struct pitc_timer adc_cal_timer = {
+	.interval = 1,
+	.func = &adc_calibrate_cb,
+};
+
+void adc_trigger(void)
+{
+	printf("R:%4d:%4d N:%4d:%4d Y:%4d:%4d U:%4d\n\r",
+		adc_result[ADC_GYRO_ROLL], adc_result[ADC_ACC_ROLL],
+		adc_result[ADC_GYRO_NICK], adc_result[ADC_ACC_NICK],
+		adc_result[ADC_GYRO_YAW], adc_result[ADC_ACC_YAW],
+		adc_result[ADC_VOLTAGE]);
+
+	*AT91C_ADC_CR = AT91C_ADC_START;
+}
+
+void adc_calibrate(uint32_t mode)
+{
+	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);
 	}
 }
 
@@ -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;