Develop to master

lib/Espfc/src/Actuator.h

@@ -21,8 +21,9 @@ class Actuator
     int update()
     {
       Stats::Measure(_model.state.stats, COUNTER_ACTUATOR);
-      updateArming();
+      updateArmingDisabled();
       updateModeMask();
+      updateArmed();
       updateAirMode();
       updateScaler();
       updateBuzzer();
@@ -73,7 +74,7 @@ class Actuator
       }
     }
 
-    void updateArming()
+    void updateArmingDisabled()
     {
       int errors = _model.state.i2cErrorDelta;
       _model.state.i2cErrorDelta = 0;
@@ -119,11 +120,13 @@ class Actuator
 
       for(size_t i = 0; i < MODE_COUNT; i++)
       {
-        bool next = newMask & (1 << i);
-        bool prev = _model.state.modeMaskPrev & (1 << i);
-        if(next == prev) continue; // mode unchanged
-        if(next && canActivateMode((FlightMode)i)) continue; // mode can be set
-        newMask &= ~(1 << i); // block activation, clear bit
+        bool newVal = newMask & (1 << i);
+        bool oldVal = _model.state.modeMask & (1 << i);
+        if(newVal == oldVal) continue; // mode unchanged
+        if(newVal && !canActivateMode((FlightMode)i))
+        {
+          newMask &= ~(1 << i); // block activation, clear bit
+        }
       }
 
       _model.updateModes(newMask);
@@ -144,9 +147,25 @@ class Actuator
       }
     }
 
+    void updateArmed()
+    {
+      if((_model.hasChanged(MODE_ARMED)))
+      {
+        bool armed = _model.isModeActive(MODE_ARMED);
+        if(armed)
+        {
+          _model.state.disarmReason = DISARM_REASON_SYSTEM;
+        }
+        else if(!armed && _model.state.disarmReason == DISARM_REASON_SYSTEM)
+        {
+          _model.state.disarmReason = DISARM_REASON_SWITCH;
+        }
+      }
+    }
+
     void updateAirMode()
     {
-      bool armed = _model.isActive(MODE_ARMED);
+      bool armed = _model.isModeActive(MODE_ARMED);
       if(!armed)
       {
         _model.state.airmodeAllowed = false;
@@ -159,21 +178,21 @@ class Actuator
 
     void updateBuzzer()
     {
-      if(_model.isActive(MODE_FAILSAFE))
+      if(_model.isModeActive(MODE_FAILSAFE))
       {
         _model.state.buzzer.play(BEEPER_RX_LOST);
       }
       if(_model.state.battery.warn(_model.config.vbatCellWarning))
       {
         _model.state.buzzer.play(BEEPER_BAT_LOW);
       }
-      if(_model.isActive(MODE_BUZZER))
+      if(_model.isModeActive(MODE_BUZZER))
       {
         _model.state.buzzer.play(BEEPER_RX_SET);
       }
       if((_model.hasChanged(MODE_ARMED)))
       {
-        _model.state.buzzer.push(_model.isActive(MODE_ARMED) ? BEEPER_ARMING : BEEPER_DISARMING);
+        _model.state.buzzer.push(_model.isModeActive(MODE_ARMED) ? BEEPER_ARMING : BEEPER_DISARMING);
       }
     }
 

lib/Espfc/src/Blackbox.h

@@ -8,6 +8,7 @@
 
 extern "C" {
 #include "blackbox/blackbox.h"
+#include "blackbox/blackbox_fielddefs.h"
 }
 
 class BlackboxBuffer
@@ -253,9 +254,13 @@ class Blackbox
       motorConfigMutable()->minthrottle = _model.state.minThrottle;
       motorConfigMutable()->maxthrottle = _model.state.maxThrottle;
 
-      gyroConfigMutable()->gyro_sync_denom = 1; //_model.config.gyroSync;
+      gyroConfigMutable()->gyro_sync_denom = 1;
       pidConfigMutable()->pid_process_denom = _model.config.loopSync;
 
+      if(_model.accelActive()) enabledSensors |= SENSOR_ACC;
+      if(_model.magActive()) enabledSensors |= SENSOR_MAG;
+      if(_model.baroActive()) enabledSensors |= SENSOR_BARO;
+
       gyro.sampleLooptime = 125; //_model.state.gyroTimer.interval;
       targetPidLooptime = _model.state.loopTimer.interval;
       activePidLoopDenom = _model.config.loopSync;
@@ -291,9 +296,9 @@ class Blackbox
     {
       switch(e.type)
       {
-        case EVENT_MIXER_UPDATE:
+        case EVENT_MIXER_UPDATED:
           update();
-          //_model.state.appQueue.send(Event(EVENT_BBLOG_UPDATE));
+          //_model.state.appQueue.send(Event(EVENT_BBLOG_UPDATED));
           return 1;
         default:
           break;
@@ -350,25 +355,36 @@ class Blackbox
 
     void updateArmed()
     {
+      // log arming beep event
       static uint32_t beep = 0;
-      if(beep != 0 && beep < _model.state.loopTimer.last)
+      if(beep != 0 && _model.state.loopTimer.last > beep)
       {
         setArmingBeepTimeMicros(_model.state.loopTimer.last);
         beep = 0;
       }
 
-      bool armed =_model.isActive(MODE_ARMED);
+      // stop logging
+      static uint32_t stop = 0;
+      if(stop != 0 && _model.state.loopTimer.last > stop)
+      {
+        blackboxFinish();
+        stop = 0;
+      }
+
+      bool armed = _model.isActive(MODE_ARMED);
       if(armed == ARMING_FLAG(ARMED)) return;
       if(armed)
       {
         ENABLE_ARMING_FLAG(ARMED);
-        beep = _model.state.loopTimer.last + 200000; // delay arming beep event ~50ms (200ms)
+        beep = _model.state.loopTimer.last + 200000; // schedule arming beep event ~200ms
       }
       else
       {
         DISABLE_ARMING_FLAG(ARMED);
-        setArmingBeepTimeMicros(micros());
-        blackboxFinish();
+        flightLogEventData_t eventData;
+        eventData.disarm.reason = _model.state.disarmReason;
+        blackboxLogEvent(FLIGHT_LOG_EVENT_DISARM, &eventData);
+        stop = _model.state.loopTimer.last + 500000; // schedule stop in 500ms
       }
     }
 

lib/Espfc/src/Cli.h

@@ -380,11 +380,11 @@ class Cli
 
         Param(PSTR("features"), &c.featureMask),
         Param(PSTR("debug_mode"), &c.debugMode, debugModeChoices),
+        Param(PSTR("debug_axis"), &c.debugAxis),
 
         Param(PSTR("gyro_bus"), &c.gyroBus, busDevChoices),
         Param(PSTR("gyro_dev"), &c.gyroDev, gyroDevChoices),
         Param(PSTR("gyro_dlpf"), &c.gyroDlpf, gyroDlpfChoices),
-        Param(PSTR("gyro_sync"), &c.gyroSync),
         Param(PSTR("gyro_align"), &c.gyroAlign, alignChoices),
         Param(PSTR("gyro_lpf_type"), &c.gyroFilter.type, filterTypeChoices),
         Param(PSTR("gyro_lpf_freq"), &c.gyroFilter.freq),
@@ -1182,16 +1182,16 @@ class Cli
         {
           s.print(FPSTR(_model.state.stats.getName((StatCounter)i)));
           s.print(": ");
-          s.print((int)(_model.state.stats.getTime((StatCounter)i) * _model.state.loopTimer.interval), 1);
+          s.print((int)(_model.state.stats.getTime((StatCounter)i)), 1);
           s.print("us, ");
           s.print(_model.state.stats.getLoad((StatCounter)i), 1);
           s.print("%");
           s.println();
         }
         s.print(F("  TOTAL: "));
-        s.print((int)(_model.state.stats.getTotalTime() * _model.state.loopTimer.interval));
+        s.print((int)(_model.state.stats.getCpuTime()));
         s.print(F("us, "));
-        s.print(_model.state.stats.getTotalLoad(), 1);
+        s.print(_model.state.stats.getCpuLoad(), 1);
         s.print(F("%"));
         s.println();
       }

lib/Espfc/src/Controller.h

@@ -23,11 +23,16 @@ class Controller
     {
       switch(e.type)
       {
-        case EVENT_IMU_UPDATE:
-          if(_model.state.loopTimer.syncTo(_model.state.gyroTimer)) {
+        case EVENT_GYRO_READ:
+          _model.state.loopUpdate = true;
+          return 1;
+        case EVENT_IMU_UPDATED:
+          if(_model.state.loopUpdate)
+          {
             update();
+            _model.state.loopUpdate = false;
+            _model.state.appQueue.send(Event(EVENT_PID_UPDATED));
           }
-          _model.state.appQueue.send(Event(EVENT_PID_UPDATE));
           return 1;
         default:
           break;

lib/Espfc/src/Espfc.h

@@ -68,20 +68,27 @@ class Espfc
       {
         return 0;
       }
+      Stats::Measure measure(_model.state.stats, COUNTER_CPU_0);
 
       _sensor.read();
       _input.update();
-      _actuator.update();
-      _serial.update();
-      _buzzer.update();
-      _model.state.stats.update();
-
+      if(_model.state.actuatorTimer.check())
+      {
+        _actuator.update();
+      }
+      if(_model.state.serialTimer.check())
+      {
+        _serial.update();
+        _buzzer.update();
+        _model.state.stats.update();
+      }
       _model.state.appQueue.send(Event(EVENT_IDLE));
 
       return 1;
 #else
       if(_model.state.gyroTimer.check())
       {
+        Stats::Measure measure(_model.state.stats, COUNTER_CPU_0);
         _sensor.update();
         if(_model.state.loopTimer.syncTo(_model.state.gyroTimer))
         {
@@ -110,6 +117,7 @@ class Espfc
 #if defined(ESPFC_MULTI_CORE)
       Event e = _model.state.appQueue.receive();
       //Serial2.write((uint8_t)e.type);
+      Stats::Measure measure(_model.state.stats, COUNTER_CPU_1);
 
       _sensor.onAppEvent(e);
       _controller.onAppEvent(e);
@@ -118,10 +126,11 @@ class Espfc
 #else
       if(_model.state.serialTimer.check())
       {
+        Stats::Measure measure(_model.state.stats, COUNTER_CPU_1);
         _serial.update();
+        _buzzer.update();
+        _model.state.stats.update();
       }
-      _buzzer.update();
-      _model.state.stats.update();
 #endif
 
       return 1;

lib/Espfc/src/Filter.h

@@ -29,13 +29,13 @@ namespace Espfc {
 enum FilterType {
   FILTER_PT1,
   FILTER_BIQUAD,
+  FILTER_PT2,
+  FILTER_PT3,
   FILTER_NOTCH,
   FILTER_NOTCH_DF1,
   FILTER_BPF,
   FILTER_FIR2,
   FILTER_MEDIAN3,
-  FILTER_PT2,
-  FILTER_PT3,
   FILTER_NONE,
 };
 
@@ -64,11 +64,6 @@ class FilterConfig
       return FilterConfig(t, f, c);
     }
 
-    FilterConfig reconfigure(int16_t freq, int16_t cutoff = 0) const
-    {
-      return FilterConfig((FilterType)type, freq, cutoff);
-    }
-
     int8_t type;
     int16_t freq;
     int16_t cutoff;
@@ -354,10 +349,34 @@ class Filter
 
     void reconfigure(int16_t freq, int16_t cutoff = 0)
     {
-      reconfigure(_conf.reconfigure(freq, cutoff), _rate);
+      reconfigure(FilterConfig((FilterType)_conf.type, freq, cutoff), _rate);
+    }
+
+    void reconfigure(int16_t freq, int16_t cutoff, float q)
+    {
+      reconfigure(FilterConfig((FilterType)_conf.type, freq, cutoff), _rate, q);
     }
 
     void reconfigure(const FilterConfig& config, int rate)
+    {
+      _rate = rate;
+      _conf = config.sanitize(_rate);
+      switch(_conf.type)
+      {
+        case FILTER_BIQUAD:
+          reconfigure(config, rate, 0.70710678118f); // 1.0f / sqrtf(2.0f); // quality factor for butterworth lpf
+          break;
+        case FILTER_NOTCH:
+        case FILTER_NOTCH_DF1:
+        case FILTER_BPF:
+          reconfigure(config, rate, getNotchQApprox(config.freq, config.cutoff));
+          break;
+        default:
+          reconfigure(config, rate, 0.f);
+      }
+    }
+
+    void reconfigure(const FilterConfig& config, int rate, float q)
     {
       _rate = rate;
       _conf = config.sanitize(_rate);
@@ -367,14 +386,14 @@ class Filter
           _state.pt1.init(_rate, _conf.freq);
           break;
         case FILTER_BIQUAD:
-          initBiquadLpf(_rate, _conf.freq);
+          _state.bq.init(BIQUAD_FILTER_LPF, _rate, _conf.freq, q);
           break;
         case FILTER_NOTCH:
         case FILTER_NOTCH_DF1:
-          initBiquadNotch(_rate, _conf.freq, _conf.cutoff);
+          _state.bq.init(BIQUAD_FILTER_NOTCH, _rate, _conf.freq, q);
           break;
         case FILTER_BPF:
-          initBiquadBpf(_rate, _conf.freq, _conf.cutoff);
+          _state.bq.init(BIQUAD_FILTER_BPF, _rate, _conf.freq, q);
           break;
         case FILTER_FIR2:
           _state.fir2.init();
@@ -408,25 +427,6 @@ class Filter
 #if !defined(UNIT_TEST)
   private:
 #endif
-    // BIQUAD
-    void initBiquadLpf(float rate, float freq)
-    {
-      const float q = 0.70710678118f;
-      //const float q = 1.0f / sqrtf(2.0f); /* quality factor: butterworth for lpf */
-      _state.bq.init(BIQUAD_FILTER_LPF, rate, freq, q);
-    }
-
-    void initBiquadNotch(float rate, float freq, float cutoff)
-    {
-      const float q = getNotchQApprox(freq, cutoff);
-      _state.bq.init(BIQUAD_FILTER_NOTCH, rate, freq, q);
-    }
-
-    void initBiquadBpf(float rate, float freq, float cutoff)
-    {
-      const float q = getNotchQApprox(freq, cutoff);
-      _state.bq.init(BIQUAD_FILTER_BPF, rate, freq, q);
-    }
 
     int _rate;
     FilterConfig _conf;