callbacks.cpp

00001 #include "CommandProcessor.h"
00002 
00003 #include "defaults.h"
00004 #include "derived.h"
00005 #include "globals.h"
00006 #include "main.h"
00007 
00008 char linebuf[128];
00009 int index = 0;
00010 
00011 void rxCallback() {
00012     while (io.pc->readable()) {
00013         char c = io.pc->getc();
00014         if (c != 127 && c != 8 && c != '\r' && c != '\t') {
00015             linebuf[index] = c;
00016             if (index < 127) index++;
00017             if (c < 128) io.pc->putc(c);
00018         } else if (c == 127 || c == 8) {
00019             if (index > 0) {
00020                 index--;
00021                 //BS (8) should delete previous char
00022                 io.pc->putc(127);
00023             }
00024         } else if (c == '\r') {
00025             linebuf[index] = 0;
00026             if (!io.cmd_busy && linebuf[0] > 127) {
00027                 processCmdFast(io.pc, io.pref, linebuf);
00028             } else if (!io.cmd_busy && index > 0) {
00029                 io.pc->putc(c);
00030                 processCmd(io.pc, io.pref, linebuf);
00031                 io.pc->putc('>');
00032             } else {
00033                 io.pc->putc(c);
00034                 io.pc->putc('>');
00035             }
00036             index = 0;
00037         }
00038     }
00039 }
00040 
00041 extern "C" void TIM1_UP_TIM10_IRQHandler(void) {
00042     int start_state = io.throttle_in->state();
00043     
00044     if (TIM1->SR & TIM_SR_UIF ) {
00045         ADC1->CR2  |= 0x40000000; 
00046         volatile int delay;
00047         for (delay = 0; delay < 35; delay++);
00048         
00049         read.adval1 = ADC1->DR;
00050         read.adval2 = ADC2->DR;
00051 
00052         commutate();
00053     }
00054     TIM1->SR = 0x00;
00055     int end_state = io.throttle_in->state();
00056     if (start_state != end_state) io.throttle_in->block();
00057 }
00058 
00059 void update_velocity() {
00060     read.last_p_mech = read.p_mech;
00061     read.p_mech = io.pos->GetMechPosition();
00062     
00063     float dp_mech = read.p_mech - read.last_p_mech;
00064     if (dp_mech < -PI) dp_mech += 2 * PI;
00065     if (dp_mech >  PI) dp_mech -= 2 * PI;
00066     
00067     float w_raw = dp_mech * F_SW; //rad/s
00068     
00069     read.w = control.velocity_filter->update(w_raw);
00070 }