Diff: Radio_Control_In.cpp

Revision:

16:d1e4b9ad3b8b

Parent:

14:acaa1add097b

--- a/Radio_Control_In.cpp	Sat Nov 30 18:40:30 2019 +0000
+++ b/Radio_Control_In.cpp	Tue Jun 09 09:20:19 2020 +0000
@@ -1,6 +1,7 @@
 #include "mbed.h"
 #include "BufferedSerial.h"
 #include "Radio_Control_In.h"
+#include "STM3_ESC.h"
 /**class   RControl_In
     Jon Freeman
     Jan 2019
@@ -9,6 +10,7 @@
     Checks repetition rate in range 5-25ms
 */
 extern  BufferedSerial  pc;
+//extern  eeprom_settings     user_settings     ;
 
 //    RControl_In::RControl_In   ()   {    //  Default Constructor
 //        pulse_width_us = period_us = pulse_count = 0;
@@ -39,29 +41,103 @@
     return  period_us;
 }
 
-void    RControl_In::reset ()
-{
-    pulse_width_us = period_us = pulse_count = 0;
-    t.reset ();
-}
-
 bool    RControl_In::validate_rx ()
 {   //  Tests for pulse width and repetition rates being believable
     return  !((period_us < 5000) || (period_us > 25000) || (pulse_width_us < 800) || (pulse_width_us > 2200));
 }
 
-double  RControl_In::normalised  ()  {
-    if  (!validate_rx())    {
-        pc.printf   ("Invalid RCin\r\n");
-        return  lost_chan_return_value;     //  ** WHAT TO RETURN WHEN RC NOT ACTIVE ? ** This is now user settable
-    }                                       //  Defaults to returning 0.0, user should call set_lost_chan_value (value) to alter
-    double norm = (double) (pulse_width_us - 1000);  //  left with -200 to + 1200 allowing for some margin
-    norm /= 1000.0;
-    if  (norm > 1.0)
-        norm = 1.0;
-    if  (norm < 0.0)
-        norm = 0.0;
-    return  norm;
+bool        RControl_In::energise    (struct  RC_stick_info & stick, struct brushless_motor & motor)  {     //  December 2019
+    if  (stick.active)   {
+        if  (stick.zone == ZONE_DRIVE)   {
+            motor.set_mode (stick.stick_implied_motor_direction == 1 ? MOTOR_FORWARD : MOTOR_REVERSE);
+            motor.set_V_limit  (stick.drive_effort);
+            motor.set_I_limit  (stick.drive_effort);    //  This could be 1.0, or other options
+        }
+        if  (stick.zone == ZONE_BRAKE)   {
+            motor.brake    (stick.brake_effort);
+        }
+    }
+    return  stick.active;
+}
+
+bool        RControl_In::read    (class  RC_stick_info & stick)  {      //  December 2019
+    double  dtmp;
+    uint32_t    old_zone = stick.zone;
+    stick.chan_mode = get_chanmode(); //  0 disabled, 1 uni-dir, or 2 bi-dir
+    stick.active = validate_rx();     //  True if RC Rx delivering believable pulse duration and timing
+    if  (stick.active && (stick.chan_mode < 1 || stick.chan_mode > 2))    {   //  Should signal an error here
+        stick.active = false;
+    }
+    if  (stick.active)    {
+        stick.raw = (double) (pulse_width_us - 1000);  //  Read pulse width from Rx, left with -200.0 to + 1200.0 allowing for some margin
+        stick.raw /= 1000.0;        //  pulse width varies between typ 1000 to 2000 micro seconds
+        stick.raw += range_offset;  //  range now normalised to 0.0 <= raw <= 1.0
+        if  (stick.raw > 1.0)        stick.raw = 1.0;
+        if  (stick.raw < 0.0)        stick.raw = 0.0;   //  clipped to strict limits 0.0 and 1.0
+        if  (stick_sense != 0)
+            stick.raw = 1.0 - stick.raw;    //  user setting allows for stick sense reversal
+        stick.deflection = stick.raw;
+        stick.stick_implied_motor_direction = +1;     //  -1 Reverse, 0 Stopped, +1 Forward
+        if  (stick.chan_mode == 2)  {       //  Bi-directional centre zero stick mode selected by user
+            stick.deflection = (stick.raw * 2.0) - 1.0;     //  range here -1.0 <= deflection <= +1.0
+            if  (stick.deflection < 0.0)    {
+                stick.deflection = 0.0 - stick.deflection;  //  range inverted if negative, direction info separated out
+                stick.stick_implied_motor_direction = -1;     //  -1 Reverse, 0 Stopped, +1 Forward (almost never 0)
+            }   //  endof deflection < 0.0
+        }       //  endof if chan_mode == 2
+        //  Now find zone from deflection
+        stick.zone = ZONE_COAST;
+        if  (stick.deflection < (brake_segment - 0.02))     //  size of brake_segment user settable
+            stick.zone = ZONE_BRAKE;
+        if  (stick.deflection > (brake_segment + 0.02))     //  Tiny 'freewheel' COAST band between drive and brake
+            stick.zone = ZONE_DRIVE;
+        if  (old_zone != ZONE_COAST && old_zone != stick.zone)    //
+            stick.zone = ZONE_COAST;        //  Ensures transitions between BRAKE and DRIVE go via COAST
+        switch  (stick.zone)    {
+            case    ZONE_COAST:
+                stick.drive_effort = 0.0;
+                stick.brake_effort = 0.0;
+                break;
+            case    ZONE_BRAKE:
+                stick.brake_effort = (brake_segment - stick.deflection) / brake_segment;    //  1.0 at zero deflection, reducing to 0.0 on boundary with DRIVE
+                stick.drive_effort = 0.0;
+                break;
+            case    ZONE_DRIVE:
+                stick.brake_effort = 0.0;
+                dtmp = (stick.deflection - brake_segment) / (1.0 - brake_segment);
+                if  (dtmp > stick.drive_effort) {       //  Stick has moved in increasing demand direction
+                    stick.drive_effort *= (1.0 - stick_attack);     //  Apply 'viscous damping' to demand increases for smoother operation
+                    stick.drive_effort += (dtmp * stick_attack);    //  Low pass filter, time constant variable by choosing 'stick_attack' value %age
+                }
+                else    //  Reduction or no increase in demanded drive effort
+                    stick.drive_effort = dtmp;      //  Reduce demand immediately, i.e. no viscous damping on reduced demand
+                break;
+        }       //  endof switch
+    }           //  endof if active
+    else    {   //  stick Not active
+        stick.zone = ZONE_BRAKE;
+        stick.raw = 0.0;
+        stick.deflection = 0.0;
+    }           //  endof not active
+    return  stick.active;
+}
+
+
+void    RControl_In::set_offset (signed char offs, char brake_pcent, char attack)   {   //  Takes user_settings[RCIx_TRIM]
+    brake_segment = ((double) brake_pcent) / 100.0;
+    stick_attack = ((double) attack) / 100.0;
+    range_offset = (double) offs;
+    range_offset /= 1000.0;         //  This is where to set range_offset sensitivity
+//    pc.printf   ("In RControl_In::set_offset, input signed char = %d, out f %.3f\r\n", offs, range_offset);
+}
+
+uint32_t    RControl_In::get_chanmode    () {
+    return  chan_mode;
+}
+
+void    RControl_In::set_chanmode    (char c, char polarity) {
+    chan_mode = ((uint32_t) c);
+    stick_sense = (uint32_t) polarity;
 }
 
 void    RControl_In::RadC_fall    ()     //  December 2018 - Could not make Servo port bidirectional, fix by using PC_14 and 15 as inputs