rpm.cpp

00001 #include "mbed.h"
00002 #include "rpm.h"
00003 #include "Alternator.h"
00004 //#include "BufferedSerial.h"
00005 //extern  BufferedSerial pc;
00006 extern  ee_settings_2020 user_settings;
00007 
00008 Engine_Manager::Engine_Manager    (PinName ms, PinName cm, PinName for_servo) : MagnetoSignal(ms), CleanedMagneto(cm), Speed_ControlServo(for_servo)
00009 {
00010     //      Constructor
00011     magneto_stretch     = false;
00012     rpm_in_run_range    = false;
00013     latest_RPM_reading = core_call_count = 0;
00014     last_update_time = 0;
00015     filtered_measured_RPM = requested_RPM = 0.0;
00016     MagnetoSignal.rise  (callback(this, &Engine_Manager::MagRise));     // Attach handler to the rising interruptIn edge
00017     MagnetoSignal.fall  (callback(this, &Engine_Manager::MagFall));     // Attach handler to the rising interruptIn edge
00018 //    microsecs.reset  ()  ;   //  timer = 0
00019 //    microsecs.start  ()  ;   //  64 bit, counts micro seconds and times out in half million years
00020     servo_position = MIN_WORKING_THROTTLE;
00021     Set_Speed_Lever    (MIN_WORKING_THROTTLE);
00022 };
00023 
00024 void        Engine_Manager::magneto_timeoutC    ()
00025 {
00026     magneto_stretch = false;    //  Magneto ringing finished by now, re-enable magneto pulse count
00027     CleanedMagneto  = 0;    //  Cleaned tacho output brought out to pin to look at with scope
00028 }
00029 
00030 void        Engine_Manager::MagRise    ()
00031 {
00032     if  (!magneto_stretch) {    //  May get this interrupt more than once per magneto pulse, respond to first, lock out subsequent
00033         //  until magneto_timeout time has elapsed
00034         magneto_stretch = true;     //  Set flag preventing repeat interrupts until after Timeout interrupt handler clears it 5ms later
00035         new_time = microsecs.read_high_resolution_us();
00036         magt0 = new_time - magt1;    //  microsecs between most recent two sparks
00037         magt1 = new_time;    //  actual time microsecs of most recent spark
00038         magneto_timoC.attach_us (callback(this, &Engine_Manager::magneto_timeoutC), DEBOUNCE_US);    //  To ignore ringing and multiple counts on magneto output, all settled within about 5ms
00039         CleanedMagneto  = 1;    //  Cleaned tacho output brought out to pin to look at with scope
00040     }
00041 }
00042 
00043 void        Engine_Manager::MagFall    ()     //  Need only rising or falling edge, not both
00044 {
00045 }
00046 
00047 void        Engine_Manager::RPM_update    ()
00048 {
00049     last_update_time = microsecs.read_high_resolution_us();
00050     time_since_last_spark = last_update_time - magt1;
00051     if  (time_since_last_spark > 500000)    {     //  engine probably not running
00052         latest_RPM_reading = 0;
00053         running_flag = false;
00054     } 
00055     else    {
00056         running_flag = true;
00057         latest_RPM_reading = (60000000 / magt0);
00058     }
00059     filtered_measured_RPM *= (1.0 - RPM_FILTER_FACTOR);
00060     filtered_measured_RPM += RPM_FILTER_FACTOR * (double)latest_RPM_reading;
00061 }
00062 
00063 bool        Engine_Manager::running   ()    //  Answers question is engine running?
00064 {
00065     return  running_flag;
00066 }
00067 
00068 uint32_t    Engine_Manager::RPM_latest    ()
00069 {
00070     return  latest_RPM_reading;
00071 }
00072 
00073 uint32_t    Engine_Manager::RPM_filtered  ()
00074 {
00075     return  (uint32_t)filtered_measured_RPM;
00076 }
00077 
00078 void        Engine_Manager::Set_Speed_Lever  (double th)       //  Make this private once all throttle refs local
00079 {
00080     if  (user_settings.rd(SERVO_DIR) == 0)
00081         Speed_ControlServo   = th;
00082     else
00083         Speed_ControlServo = 1.0 - th;
00084 }
00085 
00086 uint32_t    Engine_Manager::set_RPM_literal (uint32_t new_rpm)        //  Returns latest measured RPM
00087 {
00088     if  (new_rpm > MAX_RPM)
00089         new_rpm = MAX_RPM;
00090     if  (new_rpm >= MIN_RPM) {   //  Runnable or make runnable
00091         if  (!rpm_in_run_range) {   //  Transition from idle to working revs
00092             rpm_in_run_range = true;
00093             servo_position  = MIN_WORKING_THROTTLE; //  set throttle to probably somewhere near low useful revs
00094             Set_Speed_Lever    (MIN_WORKING_THROTTLE);
00095         }   //  Endof transition from idle to working revs
00096     }       //  Endof Runnable or make runnable
00097     else    {   //  requested rpm are below useful, so kill engine
00098         if  (rpm_in_run_range)  {   //  detect transition from useful work to idle
00099             servo_position = 0.0;
00100             Set_Speed_Lever    (0.0);
00101             rpm_in_run_range = false;
00102         }
00103     }
00104     requested_RPM = (double) new_rpm;
00105     return  latest_RPM_reading;
00106 }
00107 
00108 uint32_t    Engine_Manager::set_RPM_percent (uint32_t new_percent)
00109 {               //  Returns latest measured RPM
00110     uint32_t    temp = 0;
00111     if  (new_percent > 100) new_percent = 100;
00112     if  (new_percent != 0) 
00113         temp = MIN_RPM + (((MAX_RPM - MIN_RPM) * new_percent) / 100);
00114     return  set_RPM_literal (temp);
00115 }
00116 
00117 uint32_t    Engine_Manager::RPM_percent_to_actual (uint32_t new_percent)
00118 {               //  Returns RPM percentage converted to actual RPM
00119     uint32_t    temp = 0;
00120     if  (new_percent > 100) new_percent = 100;
00121     if  (new_percent != 0) 
00122         temp = MIN_RPM + (((MAX_RPM - MIN_RPM) * new_percent) / 100);
00123     return  temp;
00124 }
00125 
00126 uint32_t    Engine_Manager::get_RPM_requested ()        //  Returns latest requested RPM
00127 {
00128     return  (uint32_t) requested_RPM;
00129 }
00130 
00131 double      Engine_Manager::get_servo_position  ()  //  Only call from test printf in main
00132 {
00133     return  servo_position;
00134 }
00135 
00136 void        Engine_Manager::manager_core()      //  Call this at 100 Hz
00137 {
00138     uint32_t    i = ++core_call_count;
00139     int32_t     rpm_error;      //  signed
00140     RPM_update  ();     //  Updates filtered and unfiltered RPM measured, 100 times per sec
00141     i &= 7;
00142     switch  (i)  {
00143         case    1:   //  Doing this at 12.5 Hz
00144             rpm_error = filtered_measured_RPM - requested_RPM;    //  all doubles. Speed error is positive
00145             if  (rpm_in_run_range && (abs(rpm_error) > RPM_DEADBAND))  { //  Try to adjust speed if not within margin
00146                 servo_position -= rpm_error / RPM_ERR_DENOM; //Denominator sets gain, the 'P', Proportional as in PID
00147                 if  (servo_position < MIN_WORKING_THROTTLE)     //  Keep servo_position within limits
00148                     servo_position = MIN_WORKING_THROTTLE;
00149                 if  (servo_position > MAX_WORKING_THROTTLE)
00150                     servo_position = MAX_WORKING_THROTTLE;
00151                 Set_Speed_Lever    (servo_position);   //  Update signal to engine throttle control servo
00152             }
00153             break;
00154         default:
00155             break;
00156     }
00157 }
00158 
00159 
00160 
00161