Radio_Control_In.cpp

00001 #include "mbed.h"
00002 #include "BufferedSerial.h"
00003 #include "Radio_Control_In.h"
00004 #include "STM3_ESC.h"
00005 /**class   RControl_In
00006     Jon Freeman
00007     Jan 2019
00008 
00009     Checks for __-__ duration 800-2200us
00010     Checks repetition rate in range 5-25ms
00011 */
00012 extern  BufferedSerial  pc;
00013 //extern  eeprom_settings     user_settings     ;
00014 
00015 //    RControl_In::RControl_In   ()   {    //  Default Constructor
00016 //        pulse_width_us = period_us = pulse_count = 0;
00017 //        lost_chan_return_value = 0.0;
00018 //    }   ;
00019 //    RControl_In::RControl_In   (PinName inp) : pulse_in(inp)   {    //  Default Constructor
00020 //        pulse_width_us = period_us = pulse_count = 0;
00021 //        lost_chan_return_value = 0.0;
00022 //    }   ;
00023 /**
00024 */
00025 void        RControl_In::set_lost_chan_return_value  (double d)  {
00026     lost_chan_return_value = d;
00027 }
00028 
00029 uint32_t    RControl_In::pulsewidth   ()
00030 {
00031     return  pulse_width_us;
00032 }
00033 
00034 uint32_t    RControl_In::pulsecount   ()
00035 {
00036     return  pulse_count;
00037 }
00038 
00039 uint32_t    RControl_In::period   ()
00040 {
00041     return  period_us;
00042 }
00043 
00044 bool    RControl_In::validate_rx ()
00045 {   //  Tests for pulse width and repetition rates being believable
00046     return  !((period_us < 5000) || (period_us > 25000) || (pulse_width_us < 800) || (pulse_width_us > 2200));
00047 }
00048 
00049 bool        RControl_In::energise    (struct  RC_stick_info & stick, struct brushless_motor & motor)  {     //  December 2019
00050     if  (stick.active)   {
00051         if  (stick.zone == ZONE_DRIVE)   {
00052             motor.set_mode (stick.stick_implied_motor_direction == 1 ? MOTOR_FORWARD : MOTOR_REVERSE);
00053             motor.set_V_limit  (stick.drive_effort);
00054             motor.set_I_limit  (stick.drive_effort);    //  This could be 1.0, or other options
00055         }
00056         if  (stick.zone == ZONE_BRAKE)   {
00057             motor.brake    (stick.brake_effort);
00058         }
00059     }
00060     return  stick.active;
00061 }
00062 
00063 bool        RControl_In::read    (class  RC_stick_info & stick)  {      //  December 2019
00064     double  dtmp;
00065     uint32_t    old_zone = stick.zone;
00066     stick.chan_mode = get_chanmode(); //  0 disabled, 1 uni-dir, or 2 bi-dir
00067     stick.active = validate_rx();     //  True if RC Rx delivering believable pulse duration and timing
00068     if  (stick.active && (stick.chan_mode < 1 || stick.chan_mode > 2))    {   //  Should signal an error here
00069         stick.active = false;
00070     }
00071     if  (stick.active)    {
00072         stick.raw = (double) (pulse_width_us - 1000);  //  Read pulse width from Rx, left with -200.0 to + 1200.0 allowing for some margin
00073         stick.raw /= 1000.0;        //  pulse width varies between typ 1000 to 2000 micro seconds
00074         stick.raw += range_offset;  //  range now normalised to 0.0 <= raw <= 1.0
00075         if  (stick.raw > 1.0)        stick.raw = 1.0;
00076         if  (stick.raw < 0.0)        stick.raw = 0.0;   //  clipped to strict limits 0.0 and 1.0
00077         if  (stick_sense != 0)
00078             stick.raw = 1.0 - stick.raw;    //  user setting allows for stick sense reversal
00079         stick.deflection = stick.raw;
00080         stick.stick_implied_motor_direction = +1;     //  -1 Reverse, 0 Stopped, +1 Forward
00081         if  (stick.chan_mode == 2)  {       //  Bi-directional centre zero stick mode selected by user
00082             stick.deflection = (stick.raw * 2.0) - 1.0;     //  range here -1.0 <= deflection <= +1.0
00083             if  (stick.deflection < 0.0)    {
00084                 stick.deflection = 0.0 - stick.deflection;  //  range inverted if negative, direction info separated out
00085                 stick.stick_implied_motor_direction = -1;     //  -1 Reverse, 0 Stopped, +1 Forward (almost never 0)
00086             }   //  endof deflection < 0.0
00087         }       //  endof if chan_mode == 2
00088         //  Now find zone from deflection
00089         stick.zone = ZONE_COAST;
00090         if  (stick.deflection < (brake_segment - 0.02))     //  size of brake_segment user settable
00091             stick.zone = ZONE_BRAKE;
00092         if  (stick.deflection > (brake_segment + 0.02))     //  Tiny 'freewheel' COAST band between drive and brake
00093             stick.zone = ZONE_DRIVE;
00094         if  (old_zone != ZONE_COAST && old_zone != stick.zone)    //
00095             stick.zone = ZONE_COAST;        //  Ensures transitions between BRAKE and DRIVE go via COAST
00096         switch  (stick.zone)    {
00097             case    ZONE_COAST:
00098                 stick.drive_effort = 0.0;
00099                 stick.brake_effort = 0.0;
00100                 break;
00101             case    ZONE_BRAKE:
00102                 stick.brake_effort = (brake_segment - stick.deflection) / brake_segment;    //  1.0 at zero deflection, reducing to 0.0 on boundary with DRIVE
00103                 stick.drive_effort = 0.0;
00104                 break;
00105             case    ZONE_DRIVE:
00106                 stick.brake_effort = 0.0;
00107                 dtmp = (stick.deflection - brake_segment) / (1.0 - brake_segment);
00108                 if  (dtmp > stick.drive_effort) {       //  Stick has moved in increasing demand direction
00109                     stick.drive_effort *= (1.0 - stick_attack);     //  Apply 'viscous damping' to demand increases for smoother operation
00110                     stick.drive_effort += (dtmp * stick_attack);    //  Low pass filter, time constant variable by choosing 'stick_attack' value %age
00111                 }
00112                 else    //  Reduction or no increase in demanded drive effort
00113                     stick.drive_effort = dtmp;      //  Reduce demand immediately, i.e. no viscous damping on reduced demand
00114                 break;
00115         }       //  endof switch
00116     }           //  endof if active
00117     else    {   //  stick Not active
00118         stick.zone = ZONE_BRAKE;
00119         stick.raw = 0.0;
00120         stick.deflection = 0.0;
00121     }           //  endof not active
00122     return  stick.active;
00123 }
00124 
00125 
00126 void    RControl_In::set_offset (signed char offs, char brake_pcent, char attack)   {   //  Takes user_settings[RCIx_TRIM]
00127     brake_segment = ((double) brake_pcent) / 100.0;
00128     stick_attack = ((double) attack) / 100.0;
00129     range_offset = (double) offs;
00130     range_offset /= 1000.0;         //  This is where to set range_offset sensitivity
00131 //    pc.printf   ("In RControl_In::set_offset, input signed char = %d, out f %.3f\r\n", offs, range_offset);
00132 }
00133 
00134 uint32_t    RControl_In::get_chanmode    () {
00135     return  chan_mode;
00136 }
00137 
00138 void    RControl_In::set_chanmode    (char c, char polarity) {
00139     chan_mode = ((uint32_t) c);
00140     stick_sense = (uint32_t) polarity;
00141 }
00142 
00143 void    RControl_In::RadC_fall    ()     //  December 2018 - Could not make Servo port bidirectional, fix by using PC_14 and 15 as inputs
00144 {                                       //  30th November 2019 - Swapped _rise and _fall as now using Schmitt inverters on input    
00145     period_us = t.read_us    ();
00146     t.reset ();
00147     t.start ();
00148 }
00149 
00150 void    RControl_In::RadC_rise    ()
00151 {
00152     pulse_width_us = t.read_us   ();
00153     pulse_count++;
00154 }
00155 //  end of RControl_In class