RCin.cpp

00001 #include "RCin.h"
00002 
00003 
00004 using namespace std;
00005 
00006 RCin::RCin(PinName pin) : pwm1(pin){
00007     pwm1.fall(callback(this, &RCin::fall_edge));
00008     pwm1.rise(callback(this, &RCin::rise_edge));
00009     local_ti.start();
00010     local_ti.reset();
00011     }
00012 
00013 // *****************************************************************************
00014 void RCin::map_Channels(void){
00015     CH1_2_PM1.setup(600.0,1400.0,-1.0,1.0);
00016     CH2_2_PM1.setup(1400.0,600.0,-1.0,1.0);
00017     CH3_2_PM1.setup(600.0,1400.0,-1.0,1.0);
00018     CH4_2_PM1.setup(600.0,1400.0,-1.0,1.0);
00019     }
00020 
00021 // *****************************************************************************
00022 void RCin::rise_edge(void)
00023     {
00024      local_ti.reset();
00025      }
00026 
00027 // *****************************************************************************
00028 void RCin::fall_edge(void)
00029    {
00030         uint32_t time_us = local_ti.read_us();
00031         if(time_us > 2500){
00032             if(cou<200)
00033                 cou++;
00034             chnr = 0;
00035             map_pwm_2_PM1();
00036         }
00037         else
00038         {
00039             pwms[++chnr] = time_us;
00040             test_pwms[cou][chnr-1]=time_us;
00041         }
00042     }
00043 // *****************************************************************************
00044 uint8_t RCin::get_flightmode(void){
00045     if(isAlive){
00046         flightmode_changed = (old_flightmode != current_flightmode);
00047         old_flightmode = current_flightmode;
00048         return current_flightmode;
00049         }
00050     else 
00051         return 0;
00052     }
00053 // *****************************************************************************
00054 void RCin::map_pwm_2_PM1(void){
00055     PM1[1]=CH1_2_PM1((float)pwms[1]);
00056     PM1[2]=CH2_2_PM1((float)pwms[2]);
00057     PM1[3]=CH3_2_PM1((float)pwms[3]);
00058     PM1[4]=CH4_2_PM1((float)pwms[4]);
00059     if(pwms[5] >620 && pwms[5] < 720)
00060         current_flightmode = LOITER;
00061     else if(pwms[5] >720 && pwms[5] < 850)
00062         current_flightmode = AUTO;
00063     else if(pwms[5] >870 && pwms[5] < 970)
00064         current_flightmode = RTL;
00065     else if(pwms[5] >1000 && pwms[5] < 1100)
00066         current_flightmode = STABILIZED;
00067     else if(pwms[5] >1100 && pwms[5] < 1200)
00068         current_flightmode = ACRO;
00069     else if(pwms[5] >1200 && pwms[5] < 1350)
00070         current_flightmode = LAND;
00071     else
00072         current_flightmode = STABILIZED;
00073     if(pwms[3]>550)
00074         isAlive = true;
00075 
00076     }