// BSB 20120810: Added Pascal-style readkey() for polling UART
char readkey(void) {
if (usart_test_hit(DBG_USART)) // A character is waiting in RX buffer
return 1;
else
return 0; // Report no character waiting
}
// BSB 20120810: Added rtos_delay
char read_dbg_char(char echo, char rtos_delay, char checksum_mode)
{
volatile static char dbg_checksum = 0; // should be uint8_t??
char read_data; // should be uint8_t??
// dbg_checksum is a crude checksum mechanism compatible by other debug code by BSB.
if (checksum_mode == DBG_CHECKSUM_NORMAL)
{
// Redirection to the debug USART.
if (rtos_delay == RTOS_WAIT) { // Wait for uart RX register to fill.
while (!usart_test_hit(DBG_USART))
vTaskDelay(120); // Giving 12ms to RTOS all the while
}
read_data = usart_getchar(DBG_USART); // returns int
if (echo == DBG_ECHO)
usart_putchar(DBG_USART, read_data);
dbg_checksum += read_data; // Checksum function is addition...
dbg_checksum &= 0xFF; // ... of which we save 8 lsbs. Redundant code line?
return read_data;
}
else if (checksum_mode==DBG_CHECKSUM_RESET)
dbg_checksum = 0;
// Last alternative, DBG_CHECKSUM_READOUT, not tested
return dbg_checksum;
}
/* ISR for serial interrupt */
void rt_hw_serial_isr(void)
{
struct avr32_serial_device *uart = (struct avr32_serial_device *) _rt_usart_device.user_data;
rt_base_t level;
if (usart_test_hit(uart->uart_device))
{
/* interrupt mode receive */
RT_ASSERT(device->flag & RT_DEVICE_FLAG_INT_RX);
/* disable interrupt */
level = rt_hw_interrupt_disable();
/* save character */
uart->int_rx->rx_buffer[uart->int_rx->save_index] = uart->uart_device->rhr & 0xff;
uart->int_rx->save_index ++;
if (uart->int_rx->save_index >= UART_RX_BUFFER_SIZE)
uart->int_rx->save_index = 0;
/* if the next position is read index, discard this 'read char' */
if (uart->int_rx->save_index == uart->int_rx->read_index)
{
uart->int_rx->read_index ++;
if (uart->int_rx->read_index >= UART_RX_BUFFER_SIZE)
uart->int_rx->read_index = 0;
}
/* enable interrupt */
rt_hw_interrupt_enable(level);
/* invoke callback */
if (_rt_usart_device.rx_indicate != RT_NULL)
{
rt_size_t rx_length;
/* get rx length */
rx_length = uart->int_rx->read_index > uart->int_rx->save_index ?
UART_RX_BUFFER_SIZE - uart->int_rx->read_index + uart->int_rx->save_index :
uart->int_rx->save_index - uart->int_rx->read_index;
_rt_usart_device.rx_indicate(&_rt_usart_device, rx_length);
}
}
else
{
usart_reset_status(uart->uart_device);
}
}
static rt_size_t rt_serial_read (rt_device_t dev, rt_off_t pos, void *buffer, rt_size_t size)
{
rt_uint8_t *ptr;
rt_err_t err_code;
struct avr32_serial_device *uart;
ptr = buffer;
err_code = RT_EOK;
uart = (struct avr32_serial_device *)dev->user_data;
if (dev->flag & RT_DEVICE_FLAG_INT_RX)
{
/* interrupt mode Rx */
while (size)
{
rt_base_t level;
/* disable interrupt */
level = rt_hw_interrupt_disable();
if (uart->int_rx->read_index != uart->int_rx->save_index)
{
/* read a character */
*ptr++ = uart->int_rx->rx_buffer[uart->int_rx->read_index];
size--;
/* move to next position */
uart->int_rx->read_index ++;
if (uart->int_rx->read_index >= UART_RX_BUFFER_SIZE)
uart->int_rx->read_index = 0;
}
else
{
/* set error code */
err_code = -RT_EEMPTY;
/* enable interrupt */
rt_hw_interrupt_enable(level);
break;
}
/* enable interrupt */
rt_hw_interrupt_enable(level);
}
}
else
{
/* polling mode */
while ((rt_uint32_t)ptr - (rt_uint32_t)buffer < size)
{
while (usart_test_hit(uart->uart_device))
{
*ptr = uart->uart_device->rhr & 0xff;
ptr ++;
}
}
}
/* set error code */
rt_set_errno(err_code);
return (rt_uint32_t)ptr - (rt_uint32_t)buffer;
}
// main function
int main(void) {
t_cpu_time timeout;
t_cpu_time timeout_mpu;
float deltat_2;
float roll_first, pitch_first;
board_init();
hal.init(0,NULL);
hal.analogin->init();
sysclk_init();
init_sys_clocks();
float initials[3];
int16_t magnetic[3];
float yaw_first;
float soft[3],hard[3];
float desti[2];
float kp = 4.8;
float kpp = 0.7;
float kd = 0;
mpu9150.initMPU9150(FCPU_HZ);
mpu9150.initHMC58();
mpu9150.calibrate_mpu9150(initials);
int16_t myMagData[3];
hal.uartB->println("NEE ZAFERINDEN BAHSEDIYORSUUN");
float percent = 40;
uint8_t c, last_c;
uint16_t count = 0;
cpu_set_timeout(cpu_ms_2_cy(10, FOSC0), &timeout_mpu);
cpu_set_timeout(cpu_ms_2_cy(1000, FOSC0), &timeout);
hal.uartB->println("VER COSKUYU");
c = hal.uartB->read();
motor.motors_init();
while(1){
if (usart_test_hit(&AVR32_USART4)) {
hal.uartB->println("I am hit");
last_c = c;
c = hal.uartB->read();
if(c == '9') {
motor.kill_motors();
hal.uartB->println("KIRDIN BENI GOD DAMNIT");
while(1);
}
if (c == '8') {
percent += 1;
hal.uartB->print("Percent Increased to: ");
hal.uartB->println(percent);
}
if (c == '2') {
percent -= 1;
hal.uartB->print("Percent Decreased to: ");
hal.uartB->println(percent);
}
if (c == 'u') {
kpp = kpp + 0.1;
hal.uartB->print("Kpp is: ");
hal.uartB->println(kpp);
}
if (c == 'j') {
kpp = kpp - 0.1;
hal.uartB->print("Kpp is: ");
hal.uartB->println(kpp);
}
if (c == 't') {
kd = kd + 0.001;
hal.uartB->print("Kd is: ");
hal.uartB->println(kd);
}
if (c == 'g') {
kd = kd - 0.001;
hal.uartB->print("Kd is: ");
hal.uartB->println(kd);
}
if (c == 'y') {
kp = kp + 0.1;
hal.uartB->print("Kp is: ");
hal.uartB->println(kp);
}
if (c == 'h') {
kp = kp - 0.1;
hal.uartB->print("Kp is: ");
hal.uartB->println(kp);
}
c = last_c;
}
if (c == '5') {
//
if(cpu_is_timeout(&timeout_mpu)) {
cpu_stop_timeout(&timeout_mpu);
mpu9150.update();
PID_Pitch = Pitch_Controller(pitch,initials[1],kp,kpp,0,kd,0.01f);
motor.motor1_update(percent,PID_Pitch,0,0);
motor.motor3_update(percent,PID_Pitch,0,0);
cpu_set_timeout(cpu_ms_2_cy(10, FOSC0), &timeout_mpu);
}
// if(cpu_is_timeout(&timeout)) {
// cpu_stop_timeout(&timeout);
// hal.uartB->println(PID_Pitch);
//
// cpu_set_timeout(cpu_ms_2_cy(1000, FOSC0), &timeout);
// }
// float PID_Pitch = Pitch_Controller(pitch,initials[1],kp,kd,0,0,deltat);
// motor.motor1_update(percent,PID_Pitch,0,0);
// deltat =(float) cpu_cy_2_ms((Get_sys_count()),FOSC0)/1000;
// cpu_set_timeout( cpu_ms_2_cy(10000, FOSC0), &timeout);
// Set_sys_count(0);
}
}
}
