WebSVN - FlightCtrl - Blame - Rev 1477 - /branches/V0.76g-acid/servoboard/servoboard.c

Rev	Author	Line No.	Line
1455	acid	1
		2	#include <string.h>
		3	#include <avr/interrupt.h>
		4	#include <avr/eeprom.h>
		5	#include <util/delay.h>
		6	#include "servoboard.h"
		7	#include "twislave.h"
1459	acid	8	#include "uart.h"
1455	acid	9
		10
1461	acid	11	uint8_t eeprom_neutral_pos[6] EEMEM;
		12	uint16_t eeprom_pwm_limit[6] EEMEM;
1459	acid	13
1465	acid	14	uint8_t pwm_signal[6];
1475	acid	15	uint8_t volatile pwm_neutral_position[6];
		16	uint8_t volatile pwm_position[6];
		17	uint16_t volatile pwm_limit[6];
1465	acid	18	uint8_t pwm_active;
1475	acid	19	#if DEBUG_SIGNAL
		20	uint8_t volatile display_values = 0;
		21	#endif
1455	acid	22
		23	void pwm_init() {
		24
1475	acid	25	SERVODDR = (1<<SERVO1) \| (1<<SERVO2) \| (1<<SERVO3) \| (1<<SERVO4) \| (1<<SERVO5) \| (1<<SERVO6);
1455	acid	26
		27	set_pwm();
		28
		29	TCCR1B = (1 << CS11);
		30	TCCR0 = (1 << CS00);
		31
		32	}
		33
		34	void set_pwm() {
		35
		36	for(uint8_t n = 0; n < 6; n++) {
1477	acid	37	if (pwm_limit[0] != 0xffff) {
		38	pwm_signal[n] = pwm_position[n] - 127 + pwm_neutral_position[n];
		39	if (pwm_limit[n] != 0) {
		40	uint8_t l, h;
		41	l = pwm_limit[n] & 0xff;
		42	h = pwm_limit[n] >> 8;
		43	if (pwm_signal[n] < l) {
		44	pwm_signal[n] = l;
		45	}
		46	if (pwm_signal[n] > h) {
		47	pwm_signal[n] = h;
		48	}
1455	acid	49	}
		50	}
		51	}
		52
		53	}
		54
		55	void set_pwm_neutral() {
		56
		57	for(uint8_t i = 0; i < 6; i++) {
		58	pwm_position[i] = 127;
		59	}
		60	set_pwm();
		61
		62	}
		63
		64	void eeprom_init() {
		65
		66	eeprom_busy_wait();
1459	acid	67	eeprom_read_block(&pwm_neutral_position[0], &eeprom_neutral_pos, sizeof(pwm_neutral_position));
		68	eeprom_read_block(&pwm_limit[0], &eeprom_pwm_limit, sizeof(pwm_limit));
1455	acid	69
1477	acid	70	set_pwm_active();
1455	acid	71	}
		72
1477	acid	73	void set_pwm_active() {
		74
		75	// check if pwm is configured
		76	pwm_active = (pwm_limit[0] != 0xffff) ? (1 << SERVO1) : 0;
		77	pwm_active \|= (pwm_limit[1] != 0xffff) ? (1 << SERVO2) : 0;
		78	pwm_active \|= (pwm_limit[2] != 0xffff) ? (1 << SERVO3) : 0;
		79	pwm_active \|= (pwm_limit[3] != 0xffff) ? (1 << SERVO4) : 0;
		80	pwm_active \|= (pwm_limit[4] != 0xffff) ? (1 << SERVO5) : 0;
		81	pwm_active \|= (pwm_limit[5] != 0xffff) ? (1 << SERVO6) : 0;
		82
		83	}
		84
1455	acid	85	void eeprom_write() {
		86
		87	cli();
1459	acid	88	eeprom_write_block(&pwm_neutral_position[0], &eeprom_neutral_pos, sizeof(pwm_neutral_position));
		89	eeprom_write_block(&pwm_limit[0], &eeprom_pwm_limit, sizeof(pwm_limit));
1455	acid	90	sei();
		91
		92	}
		93
		94	void delay(int ms) {
		95
		96	while(ms--) {
		97	_delay_ms(1);
		98	}
		99
		100	}
		101
		102	int main(void) {
		103
1461	acid	104	uint8_t blink_counter = 0;
		105	uint8_t blink = 0;
		106
		107	// intialize
1475	acid	108	DDRD = (1<<PD7);
		109	PORTD = (1<<PD7);
1455	acid	110
1477	acid	111	delay(1000);
		112
1455	acid	113	cli();
1459	acid	114	uart_init();
1455	acid	115	eeprom_init();
1464	acid	116	i2c_init(I2C_ADDRESS);
1455	acid	117	pwm_init();
1456	acid	118	set_pwm_neutral();
1455	acid	119	sei();
		120
		121	while(1) {
		122
1475	acid	123	// start pwm
1455	acid	124	cli();
1475	acid	125	TCNT1 = 0;
1461	acid	126	SERVOPORT = pwm_active;
		127
1475	acid	128	// update signals
		129	set_pwm();
		130
1461	acid	131	// show status
		132	blink_counter++;
		133
1477	acid	134	if (pwm_active == 0) {
1461	acid	135	blink = 50;
		136	} else {
1475	acid	137	if (I2C_timeout) {
1461	acid	138	I2C_timeout--;
1466	acid	139	if (I2C_timeout == 0) { // no i2c signal
		140	blink = 0;
1465	acid	141	set_pwm_neutral();
		142	set_pwm();
1477	acid	143	} else {
		144	blink = 5;
1465	acid	145	}
1477	acid	146	} else {
		147	blink = 0;
1461	acid	148	}
		149	}
		150
		151	if (blink == 0) {
		152	LED_ON;
		153	} else if (blink_counter % blink == 0) {
		154	blink_counter = 1;
		155	if (LED_IS_ON) {
1464	acid	156	LED_OFF;
1461	acid	157	} else {
		158	LED_ON;
		159	}
		160	}
		161
1475	acid	162	// till here the code execution takes less than 70ys, plenty enough time for some more calculations
		163
		164	// wait till 300ys are reached
1455	acid	165	while(TCNT1 < 300) ;
		166
1475	acid	167	// check servo settings and set pwm to low
1455	acid	168	for(uint8_t i = 0; i < 255; i++) {
		169
		170	TCNT0 = 0;
		171
		172	if (i == pwm_signal[0]) {
		173	SERVOPORT &= ~(1<<SERVO1);
		174	}
		175	if (i == pwm_signal[1]) {
		176	SERVOPORT &= ~(1<<SERVO2);
		177	}
		178	if (i == pwm_signal[2]) {
		179	SERVOPORT &= ~(1<<SERVO3);
		180	}
		181	if (i == pwm_signal[3]) {
		182	SERVOPORT &= ~(1<<SERVO4);
		183	}
		184	if (i == pwm_signal[4]) {
		185	SERVOPORT &= ~(1<<SERVO5);
		186	}
		187	if (i == pwm_signal[5]) {
		188	SERVOPORT &= ~(1<<SERVO6);
		189	}
		190
		191	while (TCNT0 < 60) ;
		192
		193	}
		194
1461	acid	195	// set all servos to low
1455	acid	196	SERVOPORT = 0;
		197
1475	acid	198	// enable interrupts and receive data while waiting till 20ms are reached and the next pulse has to be sent
		199	sei();
1455	acid	200
1475	acid	201	#if DEBUG_SIGNAL
		202	if (display_values) {
		203
		204	cli();
		205	uart_putchar('\r');
		206	for(uint8_t i = 0; i < 6; i++) {
		207	uart_putchar(i+49);
		208	uart_putchar(':');
		209	uart_putchar((pwm_signal[i]/100)%10+48);
		210	uart_putchar((pwm_signal[i]/10)%10+48);
		211	uart_putchar((pwm_signal[i])%10+48);
		212	uart_putchar(' ');
		213	}
		214	sei();
		215
		216	}
		217	#endif
		218
1455	acid	219	while(TCNT1 < 20000) ;
		220
1475	acid	221
1455	acid	222	}
		223
		224	return 0;
		225	}
		226

Subversion Repositories FlightCtrl

(root)/branches/V0.76g-acid/servoboard/servoboard.c - Rev 1477