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;

volatile uint8_t pwm_signal[6];
volatile uint8_t pwm_neutral_position[6];
volatile uint8_t pwm_position[6];
volatile uint16_t pwm_limit[6];
volatile uint8_t pwm_active;


void pwm_init() {

        SERVODDR = 0xff;

        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) ? (2 << SERVO1) : 0;
        pwm_active |= (pwm_limit[2] != 0xffff) ? (3 << SERVO1) : 0;
        pwm_active |= (pwm_limit[3] != 0xffff) ? (4 << SERVO1) : 0;
        pwm_active |= (pwm_limit[4] != 0xffff) ? (5 << SERVO1) : 0;
        pwm_active |= (pwm_limit[5] != 0xffff) ? (6 << SERVO1) : 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 = 0x80;
        PORTD = 0x80;

        DDRC &= ~0x0f;
        PORTC |= 0x0f;

        cli();
        uart_init();
        eeprom_init();
        i2c_init(I2C_ADDRESS);
        pwm_init();
        set_pwm_neutral();
        sei();

        // start pwm
        TCNT1 = 0;
        while(1) {

                cli();
                SERVOPORT = pwm_active;

                // show status
                blink_counter++;

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

                        blink = 50;

                } else {

                        if (I2C_timeout == 0) { // no i2c signal
                                blink = 0;
                        } else {
                                I2C_timeout--;
                                blink = 5;
                        }

                }

                if (blink == 0) {
                        LED_ON;
                } else if (blink_counter % blink == 0) {
                        blink_counter = 1;
                        if (LED_IS_ON) {
                                LED_OFF;
                        } else {
                                LED_ON;
                        }
                }

                // wait with high pwm signal
                while(TCNT1 < 300) ;

                // check servos setting 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) ;

                }
                sei();

                // set all servos to low
                SERVOPORT = 0;

                // update signals
                set_pwm();

                // wait till 20ms are reached
                while(TCNT1 < 20000) ;
                TCNT1 = 0;

        }

        return 0;
}