motor + throttle limit

lib/EscDriver/src/EscDriver.h

@@ -24,6 +24,14 @@ enum EscProtocol {
 class EscDriverBase
 {
   public:
+#if defined(UNIT_TEST)
+    int begin(EscProtocol protocol, bool async, int16_t rate, int timer = 0) { return 1; }
+    void end() {}
+    int attach(size_t channel, int pin, int pulse) { return 1; }
+    int write(size_t channel, int pulse) { return 1; }
+    void apply() {}
+#endif
+
     uint16_t dshotEncode(uint16_t value)
     {
       value <<= 1;
@@ -67,8 +75,8 @@ class EscDriverBase
   #define ESC_CHANNEL_COUNT 4
   #define EscDriver EscDriverBase
 
-  #define ESC_DRIVER_MOTOR_TIMER ESC_DRIVER_TIMER1
-  #define ESC_DRIVER_SERVO_TIMER ESC_DRIVER_TIMER2
+  #define ESC_DRIVER_MOTOR_TIMER 0
+  #define ESC_DRIVER_SERVO_TIMER 0
 
 #else
 

lib/Espfc/src/Model.h

@@ -321,6 +321,19 @@ class Model
         }
       }
 
+      // sanitize throttle and motor limits
+      if(config.output.throttleLimitType < 0 || config.output.throttleLimitType >= THROTTLE_LIMIT_TYPE_MAX) {
+        config.output.throttleLimitType = THROTTLE_LIMIT_TYPE_NONE;
+      }
+
+      if(config.output.throttleLimitPercent < 1 || config.output.throttleLimitPercent > 100) {
+        config.output.throttleLimitPercent = 100;
+      }
+
+      if(config.output.motorLimit < 1 || config.output.motorLimit > 100) {
+        config.output.motorLimit = 100;
+      }
+
       // configure serial ports
       uint32_t serialFunctionAllowedMask = SERIAL_FUNCTION_MSP | SERIAL_FUNCTION_BLACKBOX | SERIAL_FUNCTION_TELEMETRY_FRSKY | SERIAL_FUNCTION_TELEMETRY_HOTT;
       uint32_t featureAllowMask = FEATURE_RX_PPM | FEATURE_MOTOR_STOP | FEATURE_TELEMETRY;// | FEATURE_AIRMODE;

lib/Espfc/src/ModelConfig.h

@@ -422,6 +422,10 @@ class OutputConfig
     int16_t maxThrottle;
     int16_t dshotIdle;
 
+    int8_t throttleLimitType = 0;
+    int8_t throttleLimitPercent = 100;
+    int8_t motorLimit = 100;
+
     OutputChannelConfig channel[OUTPUT_CHANNELS];
 };
 

lib/Espfc/src/Msp/MspProcessor.h

@@ -763,8 +763,8 @@ class MspProcessor
           r.writeU8(_model.config.input.rate[AXIS_PITCH]); // pitch rate
           r.writeU8(_model.config.input.expo[AXIS_PITCH]); // pitch expo
           // 1.41+
-          r.writeU8(0); // throtle limit type (off)
-          r.writeU8(100); // throtle limit percent (100%)
+          r.writeU8(_model.config.output.throttleLimitType); // throttle_limit_type (off)
+          r.writeU8(_model.config.output.throttleLimitPercent); // throtle_limit_percent (100%)
           //1.42+
           r.writeU16(_model.config.input.rateLimit[0]); // rate limit roll
           r.writeU16(_model.config.input.rateLimit[1]); // rate limit pitch
@@ -818,8 +818,8 @@ class MspProcessor
             // 1.41
             if(m.remain() >= 2)
             {
-              m.readU8(); // throttle_limit_type
-              m.readU8(); // throttle_limit_percent
+              _model.config.output.throttleLimitType = m.readU8(); // throttle_limit_type
+              _model.config.output.throttleLimitPercent = m.readU8(); // throttle_limit_percent
             }
             // 1.42
             if(m.remain() >= 6)
@@ -1056,7 +1056,7 @@ class MspProcessor
           // 1.42+
           r.writeU8(0); // iterm_relax_cutoff
           // 1.43+
-          r.writeU8(100); // motor_output_limit
+          r.writeU8(_model.config.output.motorLimit); // motor_output_limit
           r.writeU8(0); // auto_profile_cell_count
           r.writeU8(0); // idle_min_rpm
 
@@ -1115,7 +1115,7 @@ class MspProcessor
           }
           // 1.43+
           if (m.remain() >= 3) {
-            m.readU8(); // motor_output_limit
+            _model.config.output.motorLimit = m.readU8(); // motor_output_limit
             m.readU8(); // auto_profile_cell_count
             m.readU8(); // idle_min_rpm
           }

lib/Espfc/src/Output/Mixer.h

@@ -2,7 +2,6 @@
 #define _ESPFC_OUTPUT_MIXER_H_
 
 #include "Model.h"
-#include "Hardware.h"
 #include "Output/Mixers.h"
 #include "EscDriver.h"
 
@@ -64,12 +63,11 @@ class Mixer
       const MixerConfig& mixer = _model.state.currentMixer;
 
       updateMixer(mixer, outputs);
-      writeOutput(outputs, mixer.count);
+      writeOutput(mixer, outputs);
 
       return 1;
     }
 
-  private:
     void updateMixer(const MixerConfig& mixer, float * outputs)
     {
       Stats::Measure mixerMeasure(_model.state.stats, COUNTER_MIXER);
@@ -87,9 +85,10 @@ class Mixer
       sources[MIXER_SOURCE_RC_YAW]    = _model.state.input[AXIS_YAW];
       sources[MIXER_SOURCE_RC_THRUST] = _model.state.input[AXIS_THRUST];
 
-      sources[MIXER_SOURCE_RC_AUX1] = _model.state.input[AXIS_AUX_1];
-      sources[MIXER_SOURCE_RC_AUX2] = _model.state.input[AXIS_AUX_2];
-      sources[MIXER_SOURCE_RC_AUX3] = _model.state.input[AXIS_AUX_3];
+      for(size_t i = 0; i < 3; i++)
+      {
+        sources[MIXER_SOURCE_RC_AUX1 + i] = _model.state.input[AXIS_AUX_1 + i];
+      }
 
       for(size_t i = 0; i < OUTPUT_CHANNELS; i++)
       {
@@ -110,27 +109,27 @@ class Mixer
       }
 
       // airmode logic
-      float thrust = sources[MIXER_SOURCE_THRUST];
+      float thrust = limitThrust(sources[MIXER_SOURCE_THRUST], (ThrottleLimitType)_model.config.output.throttleLimitType, _model.config.output.throttleLimitPercent);
       if(_model.isAirModeActive())
       {
         float min = 0.f, max = 0.f;
-        for(size_t i = 0; i < OUTPUT_CHANNELS; i++)
+        for(size_t i = 0; i < mixer.count; i++)
         {
           max = std::max(max, outputs[i]);
           min = std::min(min, outputs[i]);
         }
         float range = (max - min) * 0.5f;
         if(range > 1.f)
         {
-          for(size_t i = 0; i < OUTPUT_CHANNELS; i++)
+          for(size_t i = 0; i < mixer.count; i++)
           {
             outputs[i] /= range;
           }
-          thrust = 0.f;
+          thrust = 0.f; //limitThrust(0.f);
         }
         else
         {
-          thrust = constrain(thrust, -1.f + range, 1.f - range);
+          thrust = Math::clamp(thrust, -1.f + range, 1.f - range);
         }
       }
 
@@ -151,18 +150,57 @@ class Mixer
           }
         }
         entry++;
-      }     
+      }
+
+      for(size_t i = 0; i < mixer.count; i++)
+      {
+        outputs[i] = limitOutput(outputs[i], _model.config.output.channel[i], _model.config.output.motorLimit);
+      }
+    }
+
+    float limitThrust(float thrust, ThrottleLimitType type, int8_t limit)
+    {
+      if(type == THROTTLE_LIMIT_TYPE_NONE || limit >= 100 || limit < 1) return thrust;
+
+      // thrust range is [-1, 1]
+      switch(type)
+      {
+        case THROTTLE_LIMIT_TYPE_SCALE:
+          return (thrust + 1.0f) * limit * 0.01f - 1.0f;
+        case THROTTLE_LIMIT_TYPE_CLIP:
+          return Math::clamp(thrust, -1.f, (limit * 0.02f) - 1.0f);
+        default:
+          break;
+      }
+
+      return thrust;
+    }
+
+    float limitOutput(float output, const OutputChannelConfig& occ, int limit)
+    {
+      if(limit >= 100 || limit < 1) return output;
+
+      if(occ.servo)
+      {
+        const float factor = limit * 0.01f;
+        return Math::clamp(output, -factor, factor);
+      }
+      else
+      {
+        const float factor = limit * 0.02f; // *2
+        return Math::clamp(output + 1.f, 0.f, factor) - 1.0f;
+      }
     }
 
-    void writeOutput(float * out, size_t axes)
+    void writeOutput(const MixerConfig& mixer, float * out)
     {
       Stats::Measure mixerMeasure(_model.state.stats, COUNTER_MIXER_WRITE);
 
       bool stop = _stop();
       for(size_t i = 0; i < OUTPUT_CHANNELS; i++)
       {
         const OutputChannelConfig& och = _model.config.output.channel[i];
-        if(i >= axes || stop)
+        if(i >= mixer.count || stop)
         {
           _model.state.outputUs[i] = och.servo && _model.state.outputDisarmed[i] == 1000 ? och.neutral : _model.state.outputDisarmed[i];
         }
@@ -171,19 +209,19 @@ class Mixer
           if(och.servo)
           {
             const int16_t tmp = lrintf(Math::map3(out[i], -1.f, 0.f, 1.f, och.reverse ? 2000 : 1000, och.neutral, och.reverse ? 1000 : 2000));
-            _model.state.outputUs[i] = constrain(tmp, och.min, och.max);
+            _model.state.outputUs[i] = Math::clamp(tmp, och.min, och.max);
           }
           else
           {
-            float v = constrain(out[i], -1.f, 1.f);
+            float v = Math::clamp(out[i], -1.f, 1.f);
             _model.state.outputUs[i] = lrintf(Math::map(v, -1.f, 1.f, _model.state.minThrottle, _model.state.maxThrottle));
           }
         }
       }
-      _write();
+      applyOutput();
     }
 
-    void _write()
+    void applyOutput()
     {
       for(size_t i = 0; i < OUTPUT_CHANNELS; i++)
       {

lib/Espfc/src/Output/Mixers.h

@@ -48,6 +48,13 @@ enum MixerSource {
   MIXER_SOURCE_MAX,
 };
 
+enum ThrottleLimitType {
+  THROTTLE_LIMIT_TYPE_NONE,
+  THROTTLE_LIMIT_TYPE_SCALE,
+  THROTTLE_LIMIT_TYPE_CLIP,
+  THROTTLE_LIMIT_TYPE_MAX,
+};
+
 static const size_t MIXER_RULE_MAX = 64;
 
 class MixerEntry {
@@ -63,9 +70,9 @@ class MixerConfig {
   public:
     MixerConfig(): count(0), mixes(NULL) {}
     MixerConfig(const MixerConfig& c): count(c.count), mixes(c.mixes) {}
-    MixerConfig(int8_t c, MixerEntry * m): count(c), mixes(m) {}
+    MixerConfig(uint8_t c, MixerEntry * m): count(c), mixes(m) {}
 
-    int8_t count;
+    uint8_t count;
     MixerEntry * mixes;
 };