Subversion Repositories FlightCtrl

#include <string.h>

#include <avr/interrupt.h>

#include <avr/eeprom.h>

#include <util/delay.h>

#include "servoboard.h"

#include "twislave.h"

#include "uart.h"

uint8_t eeprom_neutral_pos[6] EEMEM;

uint16_t eeprom_pwm_limit[6] EEMEM;

uint8_t pwm_signal[6];

uint8_t volatile pwm_neutral_position[6];

uint8_t volatile pwm_position[6];

uint16_t volatile pwm_limit[6];

uint8_t pwm_active;

uint8_t pwm_status = 1;

#if DEBUG_SIGNAL

uint8_t volatile display_values = 0;

#endif

void pwm_init() {

        SERVODDR = (1<<SERVO1) | (1<<SERVO2) | (1<<SERVO3) | (1<<SERVO4) | (1<<SERVO5) | (1<<SERVO6);

        set_pwm();

        TCCR1B = (1 << CS11);

        TCCR0 = (1 << CS00);

}

void set_pwm() {

        for(uint8_t n = 0; n < 6; n++) {

                pwm_signal[n] = pwm_position[n] - 127 + pwm_neutral_position[n];

                if (pwm_limit[n] != 0) {

                        uint8_t l, h;

                        l = pwm_limit[n] & 0xff;

                        h = pwm_limit[n] >> 8;

                        if (pwm_signal[n] < l) {

                                pwm_signal[n] = l;

                        }

                        if (pwm_signal[n] > h) {

                                pwm_signal[n] = h;

                        }

                }

        }

}

void set_pwm_neutral() {

        for(uint8_t i = 0; i < 6; i++) {

                pwm_position[i] = 127;

        }

        set_pwm();

}

void pwm_check_active() {

        // check if pwm is configured

        pwm_active  = (pwm_limit[0] != 0xffff) ? (1<<SERVO1) : 0;

        pwm_active |= (pwm_limit[1] != 0xffff) ? (1<<SERVO2) : 0;

        pwm_active |= (pwm_limit[2] != 0xffff) ? (1<<SERVO3) : 0;

        pwm_active |= (pwm_limit[3] != 0xffff) ? (1<<SERVO4) : 0;

        pwm_active |= (pwm_limit[4] != 0xffff) ? (1<<SERVO5) : 0;

        pwm_active |= (pwm_limit[5] != 0xffff) ? (1<<SERVO6) : 0;

}

void eeprom_init() {

        eeprom_busy_wait();

        eeprom_read_block(&pwm_neutral_position[0], &eeprom_neutral_pos, sizeof(pwm_neutral_position));

        eeprom_read_block(&pwm_limit[0], &eeprom_pwm_limit, sizeof(pwm_limit));

        pwm_check_active();

}

void eeprom_write() {

        cli();

        eeprom_write_block(&pwm_neutral_position[0], &eeprom_neutral_pos, sizeof(pwm_neutral_position));

        eeprom_write_block(&pwm_limit[0], &eeprom_pwm_limit, sizeof(pwm_limit));

        sei();

}

/*

void delay(int ms) {

        while(ms--) {

                _delay_ms(1);

        }

}

*/

int main(void) {

        uint8_t blink_counter = 0;

        uint8_t blink = 0;

        // intialize

        DDRD = (1<<PD7);

        PORTD = (1<<PD7);

        cli();

        uart_init();

        eeprom_init();

        i2c_init(I2C_ADDRESS);

        pwm_init();

        set_pwm_neutral();

        sei();

        while(1) {

                // start pwm

                cli();

                TCNT1 = 0;

                SERVOPORT = pwm_active;

                // update signals

                set_pwm();

                // show status

                blink_counter++;

                if (pwm_active == 0) { // not configured

                        blink = 50;

                        pwm_status = 1;

                } else {

                        if (I2C_timeout) {

                                I2C_timeout--;

                                if (I2C_timeout == 0) { // no i2c signal

                                        blink = 0;

                                        pwm_status = 2;

                                        set_pwm_neutral();

                                        set_pwm();

                                }

                                blink = 5;

                                pwm_status = 0;

                        }

                }

                if (blink == 0) {

                        LED_ON;

                } else if (blink_counter % blink == 0) {

                        blink_counter = 1;

                        if (LED_IS_ON) {

                                LED_OFF;

                        } else {

                                LED_ON;

                        }

                }

                // till here the code execution takes less than 70ys, plenty enough time for some more calculations

                // wait till 300ys are reached

                while(TCNT1 < 300) ;

                // check servo settings and set pwm to low

                for(uint8_t i = 0; i < 255; i++) {

                        TCNT0 = 0;

                        if (i == pwm_signal[0]) {

                                SERVOPORT &= ~(1<<SERVO1);

                        }

                        if (i == pwm_signal[1]) {

                                SERVOPORT &= ~(1<<SERVO2);

                        }

                        if (i == pwm_signal[2]) {

                                SERVOPORT &= ~(1<<SERVO3);

                        }

                        if (i == pwm_signal[3]) {

                                SERVOPORT &= ~(1<<SERVO4);

                        }

                        if (i == pwm_signal[4]) {

                                SERVOPORT &= ~(1<<SERVO5);

                        }

                        if (i == pwm_signal[5]) {

                                SERVOPORT &= ~(1<<SERVO6);

                        }

                        while (TCNT0 < 60) ;

                }

                // set all servos to low

                SERVOPORT = 0;

                // enable interrupts and receive data while waiting till 20ms are reached and the next pulse has to be sent

                sei();

#if DEBUG_SIGNAL

                if (display_values) {

                        cli();

                        uart_putchar('\r');

                        for(uint8_t i = 0; i < 6; i++) {

                                uart_putchar(i+49);

                                uart_putchar(':');

                                uart_putchar((pwm_signal[i]/100)%10+48);

                                uart_putchar((pwm_signal[i]/10)%10+48);

                                uart_putchar((pwm_signal[i])%10+48);

                                uart_putchar(' ');

                        }

                        sei();

                }

#endif

                while(TCNT1 < 20000) ;

        }

        return 0;

}