AR6210.cpp

00001 #include "mbed.h"
00002 #include "AR6210.h"
00003 // Original code: https://mbed.org/users/BlazeX/code/AR8000/
00004 // Modified by Joram Querner for Spektrum AR6210
00005 
00006 AR6210::AR6210() :
00007   ChInt0(InterruptIn(p5)), // throttle
00008   ChInt1(InterruptIn(p6)), // aileron
00009   ChInt2(InterruptIn(p7)), // elevator
00010   ChInt3(InterruptIn(p8)), // rudder
00011   ChInt4(InterruptIn(p9)), // gear
00012   ChInt5(InterruptIn(p10)) // aux
00013 {}
00014                
00015 void AR6210::init()
00016 {
00017     Time.start();
00018     
00019     for(int i= 0; i < 6; i++) {
00020         LastRise[i]= 0;
00021         dTime[i]= 1000;
00022     }
00023     
00024     ChInt0.mode(PullDown);   ChInt0.rise<AR6210>(this, &AR6210::Rise0);     ChInt0.fall<AR6210>(this, &AR6210::Fall0);
00025     ChInt1.mode(PullDown);   ChInt1.rise<AR6210>(this, &AR6210::Rise1);     ChInt1.fall<AR6210>(this, &AR6210::Fall1);
00026     ChInt2.mode(PullDown);   ChInt2.rise<AR6210>(this, &AR6210::Rise2);     ChInt2.fall<AR6210>(this, &AR6210::Fall2);
00027     ChInt3.mode(PullDown);   ChInt3.rise<AR6210>(this, &AR6210::Rise3);     ChInt3.fall<AR6210>(this, &AR6210::Fall3);
00028     ChInt4.mode(PullDown);   ChInt4.rise<AR6210>(this, &AR6210::Rise4);     ChInt4.fall<AR6210>(this, &AR6210::Fall4);
00029     ChInt5.mode(PullDown);   ChInt5.rise<AR6210>(this, &AR6210::Rise5);     ChInt5.fall<AR6210>(this, &AR6210::Fall5);
00030     
00031     update();
00032 }
00033 
00034 void AR6210::update()
00035 {
00036     //Rohdaten &#65533;hmen
00037     for(int i= 0; i<6; i++)
00038         RawChannels[i]= dTime[i];
00039     
00040     Throttle= map(dTime[0], 1095, 1910, 0.0, 1.0);
00041     Aileron= map(dTime[1], 1110, 1900, -1.0, 1.0);
00042     Elevator= map(dTime[2], 1100, 1900, -1.0, 1.0);
00043     Rudder= map(dTime[3], 1105, 1895, -1.0, 1.0);
00044     
00045     Gear= map(dTime[4], 1095, 1910, 0.0, 1.0);
00046     Aux= map(dTime[5], 1095, 1910, 0.0, 1.0);
00047 }
00048 
00049 float AR6210::map(int x, int in_min, int in_max, float out_min, float out_max)
00050 {
00051     float returnValue = (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
00052     if (returnValue < out_min)
00053         return out_min;
00054     if (returnValue > out_max)
00055         return out_max;
00056 
00057       return returnValue;
00058 }