X-Git-Url: http://git.droids-corp.org/?p=protos%2Frc_servos.git;a=blobdiff_plain;f=main.c;h=4b1bbc374a51ad532c6078059449c578b15c3fe5;hp=67661d628f230b553af226c018c803eba7c54339;hb=HEAD;hpb=0d09451fdf8906a3ce1a9d0b1f3ecb475269a1cf

diff --git a/main.c b/main.c
index 67661d6..4b1bbc3 100644
--- a/main.c
+++ b/main.c
@@ -1,3 +1,8 @@
+/*
+avrdude -p atmega168p -P usb -c avrispmkii -U lfuse:w:0xe2:m -U hfuse:w:0xdf:m -U efuse:w:0xf9:m 
+-> it failed but I answered y, then make reset and it was ok
+*/
+
 #include <aversive.h>
 #include <aversive/wait.h>
 
@@ -32,21 +37,25 @@ static struct servo servo_table[] = {
 		.command = 512,
 	},
 };
-#define NB_SERVO (sizeof(servo_table)/sizeof(*servo_table))
+#define N_SERVO (sizeof(servo_table)/sizeof(*servo_table))
+
+/* we use the first servo for PPM output if enabled */
+#define PPM (0)
 
 static uint8_t bypass;
+static uint8_t ppm_enabled;
 static volatile uint8_t done;
 static uint8_t portval;
 static uint8_t rxidx;
 
-static uint8_t icp_idx = NB_SERVO;
-static uint16_t icp_servos[NB_SERVO];
+static uint8_t icp_idx = N_SERVO;
+static uint16_t icp_servos[N_SERVO];
 static uint16_t icp_prev;
 
 static uint8_t spi_out_idx; /* current byte beeing sent */
 
-#define BYPASS_ENABLE 14
-#define BYPASS_DISABLE 15
+#define PPM_BIT    0x01
+#define BYPASS_BIT 0x02
 
 #define LED_ON() do { PORTB |= 0x02; } while(0)
 #define LED_OFF() do { PORTB &= ~0x02; } while(0)
@@ -60,9 +69,10 @@ static uint8_t spi_out_idx; /* current byte beeing sent */
  * servo value. The second byte contains the lsb of the servo value.
  *
  * Command 0 is only one byte long, it means "I have nothing to say".
- * Commands 1 to NB_SERVO+1 are to set the value of servo.
- * Command 14 is to enable bypass mode.
- * Command 15 is to disable bypass mode.
+ * Commands 1 to N_SERVO (included) are to set the value of servo.
+ * Command N_SERVO+1 is:
+ * - to enable/disable ppm generation in place of first servo.
+ * - to enable/disable bypass mode
  */
 union byte0 {
 	uint8_t u8;
@@ -120,7 +130,7 @@ static void poll_spi(void)
 		SPDR = byte1_tx.u8;
 	}
 	spi_out_idx ++;
-	if (spi_out_idx >= NB_SERVO)
+	if (spi_out_idx >= N_SERVO * 2)
 		spi_out_idx = 0;
 
 	/* RX */
@@ -148,16 +158,21 @@ static void poll_spi(void)
 
 		/* process command */
 
-		if (byte0_rx.cmd_num < NB_SERVO+1) {
-			val = (uint16_t)byte0_rx.val_msb << 7;
-			val += byte1_rx.val_lsb;
+		val = (uint16_t)byte0_rx.val_msb << 7;
+		val += byte1_rx.val_lsb;
+
+		if (byte0_rx.cmd_num < N_SERVO+1) {
 			servo_table[byte0_rx.cmd_num-1].command = val;
 		}
-		else if (byte0_rx.cmd_num == BYPASS_ENABLE) {
-			bypass = 1;
-		}
-		else if (byte0_rx.cmd_num == BYPASS_DISABLE) {
-			bypass = 0;
+		else if (byte0_rx.cmd_num == N_SERVO+1) {
+			if (val & PPM_BIT)
+				ppm_enabled = 1;
+			else
+				ppm_enabled = 0;
+			if (val & BYPASS_BIT)
+				bypass = 1;
+			else
+				bypass = 0;
 		}
 	}
 
@@ -167,7 +182,6 @@ static void poll_spi(void)
 static void poll_input_capture(void)
 {
 	uint16_t icp, diff;
-	uint8_t rising;
 
 	/* no new sample, return */
 	if ((TIFR1 & _BV(ICF1)) == 0)
@@ -177,11 +191,6 @@ static void poll_input_capture(void)
 	icp = ICR1;
 	sei();
 
-	rising = !!(TCCR1B & _BV(ICES1));
-
-	/* change the edge type */
-	TCCR1B ^= _BV(ICES1);
-
 	/* clear the flag by writing a one */
 	TIFR1 = TIFR1 | _BV(ICF1);
 
@@ -190,15 +199,17 @@ static void poll_input_capture(void)
 
 	/* a rising edge with at least 2ms of state 0 means that we
 	 * get the first servo */
-	if (rising == 1 && diff > 2000) {
+	if (diff > 3000) {
 		icp_idx = 0;
 		return;
 	}
 
 	/* get the value for the servo */
-	if (rising == 0 && icp_idx < NB_SERVO) {
+	if (icp_idx < N_SERVO) {
 		if (diff < 1000)
 			icp_servos[icp_idx] = 0;
+		else if (diff > 2023)
+			icp_servos[icp_idx] = 1023;
 		else
 			icp_servos[icp_idx] = diff - 1000;
 		icp_idx++;
@@ -217,31 +228,62 @@ static void load_timer_at(uint16_t t)
 	TIMSK1 |= _BV(OCIE1A);
 }
 
-static void do_one_servo(struct servo *s)
+static void do_servos(void)
 {
-	uint16_t t;
+	uint8_t i;
+	uint16_t t, start;
+
+	/* skip first servo if ppm is enabled */
+	if (ppm_enabled)
+		i = 1;
+	else
+		i = 0;
 
-	/* set bit */
-	done = 0;
-	//portval = PORTC | (1 << s->bit);
-	portval = (1 << s->bit);
 	t = TCNT1;
-	load_timer_at(t + 20);
+	start = t + 100;
+
+	for (; i < N_SERVO; i++) {
+
+		/* set servo and PPM bit */
+		portval = 1 << servo_table[i].bit;
+		if (ppm_enabled)
+			portval |= (1 << servo_table[PPM].bit);
+
+		done = 0;
+		load_timer_at(start);
+		while (done == 0)
+			poll();
+
+		/* reset PPM bit after 300us */
+		portval = 1 << servo_table[i].bit;
+		done = 0;
+		load_timer_at(start + 300);
+		while (done == 0)
+			poll();
+
+		start = start + 1000 + servo_table[i].command;
+	}
+
+	/* set PPM bit only for last servo */
+	portval = 0;
+	if (ppm_enabled)
+		portval |= (1 << servo_table[PPM].bit);
+
+	done = 0;
+	load_timer_at(start);
 	while (done == 0)
 		poll();
 
-	/* reset bit */
-	done = 0;
+	/* reset PPM bit after 300us */
 	portval = 0;
-	//portval = PORTC & (~(1 << s->bit));
-	load_timer_at(t + 20 + 1000 + s->command);
+	done = 0;
+	load_timer_at(start + 300);
 	while (done == 0)
 		poll();
 }
 
 int main(void)
 {
-	uint8_t i;
 	uint8_t t, diff;
 	uint8_t tmp;
 	uint8_t cnt = 10;
@@ -284,9 +326,7 @@ int main(void)
 	bypass = 0;
 	while (1) {
 		t = TCNT0;
-		for (i = 0; i < NB_SERVO; i++) {
-			do_one_servo(&servo_table[i]);
-		}
+		do_servos();
 		/* wait 20 ms */
 		while (1) {
 			diff = TCNT0 - t;