pr fixes

Author: rtlopez

lib/Espfc/src/Connect/MspProcessor.cpp

@@ -509,8 +509,8 @@ void MspProcessor::processCommand(MspMessage& m, MspResponse& r, Device::SerialD
       break;
 
     case MSP_SET_ACC_TRIM:
-      _model.config.accel.trim[0] = m.readU16(); // pitch
-      _model.config.accel.trim[1] = m.readU16(); // roll
+      _model.config.accel.trim[0] = std::min<uint16_t>(300u, m.readU16()); // pitch
+      _model.config.accel.trim[1] = std::min<uint16_t>(300u, m.readU16()); // roll
       _model.onAccChange();
       break;
 
@@ -782,7 +782,7 @@ void MspProcessor::processCommand(MspMessage& m, MspResponse& r, Device::SerialD
     case MSP_SET_ARMING_CONFIG:
       m.readU8(); // auto_disarm delay
       m.readU8(); // disarm kill switch
-      _model.config.arming.smallAngle = m.readU8(); // small angle
+      _model.config.arming.smallAngle = std::min<uint8_t>(180, m.readU8()); // small angle
       break;
 
     case MSP_RC_DEADBAND:

lib/Espfc/src/Control/Actuator.cpp

@@ -105,7 +105,7 @@ void Actuator::updateArmingDisabled()
     const float maxTiltRad = Utils::toRad(_model.config.arming.smallAngle);
     const float roll = _model.state.attitude.euler[AXIS_ROLL];
     const float pitch = _model.state.attitude.euler[AXIS_PITCH];
-    const float currentTilt = std::max(abs(roll), abs(pitch));
+    const float currentTilt = std::max(std::fabs(roll), std::fabs(pitch));
     _model.setArmingDisabled(ARMING_DISABLED_ANGLE, currentTilt > maxTiltRad);
   }
   else

lib/Espfc/src/Sensor/AccelSensor.cpp

@@ -6,7 +6,7 @@ namespace Espfc::Sensor {
 static constexpr float ESPFC_FUZZY_ACCEL_ZERO = 0.05f;
 static constexpr float ESPFC_FUZZY_GYRO_ZERO = 0.20f;
 
-AccelSensor::AccelSensor(Model& model) : _model(model) {}
+AccelSensor::AccelSensor(Model& model): _model(model) {}
 
 int AccelSensor::begin()
 {
@@ -97,33 +97,33 @@ void FAST_CODE_ATTR AccelSensor::calibrate()
 {
   switch (_model.state.accel.calibrationState)
   {
-  case CALIBRATION_IDLE:
-    _model.state.accel.adc -= _model.state.accel.bias;
-    break;
-  case CALIBRATION_START:
-    _model.state.accel.bias = VectorFloat(0, 0, ACCEL_G);
-    _model.state.accel.biasSamples = 2 * _model.state.accel.timer.rate;
-    _model.state.accel.calibrationState = CALIBRATION_UPDATE;
-    break;
-  case CALIBRATION_UPDATE:
-    _model.state.accel.bias += (_model.state.accel.adc - _model.state.accel.bias) * _model.state.accel.biasAlpha;
-    _model.state.accel.biasSamples--;
-    if (_model.state.accel.biasSamples <= 0)
-    {
-      _model.state.accel.calibrationState = CALIBRATION_APPLY;
-    }
-    break;
-  case CALIBRATION_APPLY:
-    _model.state.accel.bias.z -= ACCEL_G;
-    _model.state.accel.calibrationState = CALIBRATION_SAVE;
-    break;
-  case CALIBRATION_SAVE:
-    _model.finishCalibration();
-    _model.state.accel.calibrationState = CALIBRATION_IDLE;
-    break;
-  default:
-    _model.state.accel.calibrationState = CALIBRATION_IDLE;
-    break;
+    case CALIBRATION_IDLE:
+      _model.state.accel.adc -= _model.state.accel.bias;
+      break;
+    case CALIBRATION_START:
+      _model.state.accel.bias = VectorFloat(0, 0, ACCEL_G);
+      _model.state.accel.biasSamples = 2 * _model.state.accel.timer.rate;
+      _model.state.accel.calibrationState = CALIBRATION_UPDATE;
+      break;
+    case CALIBRATION_UPDATE:
+      _model.state.accel.bias += (_model.state.accel.adc - _model.state.accel.bias) * _model.state.accel.biasAlpha;
+      _model.state.accel.biasSamples--;
+      if (_model.state.accel.biasSamples <= 0)
+      {
+        _model.state.accel.calibrationState = CALIBRATION_APPLY;
+      }
+      break;
+    case CALIBRATION_APPLY:
+      _model.state.accel.bias.z -= ACCEL_G;
+      _model.state.accel.calibrationState = CALIBRATION_SAVE;
+      break;
+    case CALIBRATION_SAVE:
+      _model.finishCalibration();
+      _model.state.accel.calibrationState = CALIBRATION_IDLE;
+      break;
+    default:
+      _model.state.accel.calibrationState = CALIBRATION_IDLE;
+      break;
   }
 }