Error fixes and base flight logic

src/Logic/FlightController.cpp

@@ -21,6 +21,7 @@ FlightController::FlightController(
     _status = DeviceStatus::Idle;
     _checkBatteryCharge();
     stopAllRotors();
+    _initPidControllers();
     ESP_LOGI(_tag, "Flight controller initialized");
 }
 
@@ -35,9 +36,7 @@ FlightController::~FlightController() {
 
 void FlightController::tick() {
     bool hasUpdates = _sensors->tick();
-    if (hasUpdates) {
     _checkBatteryCharge();
-    }
     // TODO: don't apply pid values to motors while DeviceStatus != IsFlying
     switch (_status) {
         case DeviceStatus::Idle:
@@ -50,15 +49,23 @@ void FlightController::tick() {
             break;
         case DeviceStatus::IsBoarding:
             /* TODO: implement boarding */
+            if (hasUpdates) {
                 _boarding();
+            }
             break;
         case DeviceStatus::IsFlying:
             /* TODO: implement flying */
-            if (hasUpdates) {
-                _tailRotorController->tick();
-            }
             break;
     }
+    if (hasUpdates and (_status == DeviceStatus::IsFlying or _status == DeviceStatus::IsBoarding)) {
+        _tailRotorController->input = round(_sensors->mpuData().pitch * 10) / 10;
+        _tailRotorController->update();
+
+        _heightController->input = _sensors->flightHeight();
+        auto baseDuty = (uint32_t) round(_heightController->getResultNow());
+        _rotor1->setDuty(baseDuty);
+        _rotor2->setDuty(baseDuty);
+    }
 }
 
 ReportedDeviceState FlightController::currentDeviceState() const {
@@ -106,15 +113,15 @@ void FlightController::_takeoff() {
     // TODO: implement takeoff
     _status = DeviceStatus::IsFlying;
     _tailRotorController->enable();
+    _heightController->integral = 0.f;
+    _heightController->setpoint = _defaultFlightHeight;
 }
 
 void FlightController::_boarding() {
     if (_sensors->flightHeight() <= _defaultStopMotorHeight) {
-        // TODO: stop motors and setpoints
+        stopAllRotors();
-        _status = DeviceStatus::Idle;
-        _tailRotorController->disable();
     } else {
-        // TODO: implement boarding
+        _heightController->setpoint -= _boarding_speed;
     }
 }
 
@@ -167,6 +174,7 @@ void FlightController::setRotor3Duty(int16_t rotor3Duty) {
 }
 
 void FlightController::stopAllRotors() {
+    _tailRotorController->disable();
     setRotor1Duty(0);
     setRotor2Duty(0);
     setRotor3Duty(0);
@@ -198,3 +206,9 @@ PidSettings FlightController::pidSettings() const {
     settings.pitchController = {.p = _tailRotorController->Kp, .i = _tailRotorController->Ki, .d = _tailRotorController->Kd};
     return settings;
 }
+
+
+void FlightController::_initPidControllers() {
+    _tailRotorController->setpoint = 180.f;
+    _heightController->setLimits(0, _rotor1->maxDuty());
+}

src/Logic/FlightController.hpp

@@ -76,9 +76,15 @@ public:
 private:
     void _checkBatteryCharge();
 
-    [[deprecated]] void _takeoff();
+    void _initPidControllers();
 
-    [[deprecated]] void _boarding();
+    void _updateSetpoints();
+
+    void _updatePidControllers();
+
+    void _takeoff();
+
+    void _boarding();
 
     Sensors *_sensors = nullptr;
     SavedPidRegulator *_heightController = nullptr;
@@ -87,12 +93,17 @@ private:
     BrushedMotor *_rotor1 = nullptr;
     BrushedMotor *_rotor2 = nullptr;
     DeviceStatus _status;
+    //uint32_t _flightTimer;
+    //float _verticalVelocityOffset = 0.f; // [cm/10 ms]
+    //float _pitchVelocityOffset = 0.f; // [degrees/10 ms]
 
     static constexpr const char *_tag = "FlightController";
     static constexpr uint8_t _batteryCriticalCharge = 15;  // [%]
-    static constexpr float _defaultFlightHeight = 30.f;    // [cm]
+    static constexpr float _defaultFlightHeight = 55.f;    // [cm]
-    static constexpr float _defaultHorizontAngle = 90.f;   // [degrees]
+    static constexpr float _defaultHorizontAngle = 180.f;   // [degrees]
-    static constexpr float _defaultStopMotorHeight = 5;    // [cm]
+    static constexpr float _boarding_speed = 0.1; // [cm/10 ms]
+    static constexpr float _defaultStopMotorHeight = 15;    // [cm]
+
 
     bool _unlimitedBattery = false;  // for debug purposes
 };

src/Logic/FlightDispatcher.cpp

@@ -100,14 +100,14 @@ void FlightDispatcher::_onNewMessageReceived(char *package) {
                 case SH("p2"):
                     if (pair.type() == gson::Type::Object and pair.includes(SH("p"))
                         and pair.includes(SH("i")) and pair.includes(SH("d"))) {
-                        _flightController->setYawControllerParams(
+                        _flightController->setPitchControllerParams(
                                 pair[SH("p")].toFloat(), pair[SH("i")].toFloat(), pair[SH("d")].toFloat());
                     }
                     break;
                 case SH("p3"):
                     if (pair.type() == gson::Type::Object and pair.includes(SH("p"))
                         and pair.includes(SH("i")) and pair.includes(SH("d"))) {
-                        _flightController->setPitchControllerParams(
+                        _flightController->setYawControllerParams(
                                 pair[SH("p")].toFloat(), pair[SH("i")].toFloat(), pair[SH("d")].toFloat());
                     }
                     break;
@@ -146,21 +146,21 @@ void FlightDispatcher::_pidSettingsOpened() {
     pkg.beginObj();
 
     pkg.beginObj("p1");
-    pkg["p"] = settings.flightController.p;
+    pkg.addFloat("p", settings.flightController.p, 5);
-    pkg["i"] = settings.flightController.i;
+    pkg.addFloat("i", settings.flightController.i, 5);
-    pkg["d"] = settings.flightController.d;
+    pkg.addFloat("d", settings.flightController.d, 5);
     pkg.endObj();
 
     pkg.beginObj("p2");
-    pkg["p"] = settings.yawController.p;
+    pkg.addFloat("p", settings.pitchController.p, 5);
-    pkg["i"] = settings.yawController.i;
+    pkg.addFloat("i", settings.pitchController.i, 5);
-    pkg["d"] = settings.yawController.d;
+    pkg.addFloat("d", settings.pitchController.d, 5);
     pkg.endObj();
 
     pkg.beginObj("p3");
-    pkg["p"] = settings.pitchController.p;
+    pkg.addFloat("p", settings.yawController.p, 5);
-    pkg["i"] = settings.pitchController.i;
+    pkg.addFloat("i", settings.yawController.i, 5);
-    pkg["d"] = settings.pitchController.d;
+    pkg.addFloat("d", settings.yawController.d, 5);
     pkg.endObj();
 
     pkg.endObj();

src/Logic/PIDController.hpp

@@ -14,8 +14,8 @@ public:
         assert(rotor != nullptr);
         _rotor = rotor;
         _isEnabled = isEnabled;
-        this->_dt = dt;
+        setLimits(-_rotor->maxDuty(), _rotor->maxDuty());
-        setLimits(0, _rotor->maxDuty());
+        setDirection(REVERSE);
         _pid_timer = millis();
         ESP_LOGI(_tag, "PID %s initialized with params (%f, %f, %f)", name, Kp, Ki, Kd);
     }
@@ -23,6 +23,7 @@ public:
     void enable() {
         _isEnabled = true;
         _rotor->setDuty(0);
+        integral = 0;
     }
 
     void disable() {
@@ -30,10 +31,12 @@ public:
         _rotor->setDuty(0);
     }
 
-    void tick() {
+    void update() {
-        if (millis() - _pid_timer >= _dt and _isEnabled) {
+        if (_isEnabled) {
-            _rotor->setDuty((uint16_t) getResultTimer());
+            //ESP_LOGI(_tag, "Setpoint: %f", setpoint);
-            _pid_timer = millis();
+            //ESP_LOGI(_tag, "Input: %f", input);
+            //ESP_LOGI(_tag, "Output: %d", (int16_t) round(getResultNow()));
+            setRotorDuty((int16_t) round(getResultNow()));
         }
     }
 
@@ -43,7 +46,7 @@ public:
     }
 
 private:
-    uint16_t _dt;
+    //uint16_t _dt;
     BrushedMotor *_rotor = nullptr;
     uint32_t _pid_timer = 0;
     bool _isEnabled = false;

src/Motor/BrushedMotor.cpp

@@ -23,6 +23,10 @@ void BrushedMotor::setDuty(uint32_t duty) const {
     ledcWrite(_channel, duty);
 }
 
+void BrushedMotor::decrement(uint32_t k) const {
+    ledcWrite(_channel, ledcRead(_channel) - k);
+}
+
 void BrushedMotor::setExtendedDuty(int32_t duty) {
     if (duty > 0) {
         forward();

src/Motor/BrushedMotor.hpp

@@ -27,6 +27,8 @@ public:
 
     void coast();
 
+    void decrement(uint32_t k) const;
+
 private:
     uint8_t _channel;
     uint32_t _frequency;

src/RF/BluetoothDispatcher.cpp

@@ -16,6 +16,7 @@ BluetoothDispatcher::BluetoothDispatcher(BluetoothSerial *controller, const char
 }
 
 bool BluetoothDispatcher::initialize(int readTimeoutMS) {
+    bool success = _controller->begin(_device_name, false);
     _controller->enableSSP();
     _controller->onConfirmRequest([this](uint16_t pin) {
         _onConfirmRequest(pin);
@@ -27,8 +28,7 @@ bool BluetoothDispatcher::initialize(int readTimeoutMS) {
     deviceConnectedCallback = [this](BTAddress device) {
         _onDeviceConnected(device);
     };
-    _controller->setTimeout(readTimeoutMS);
+    //_controller->setTimeout(readTimeoutMS);
-    bool success = _controller->begin(_device_name, false);
     if (!success) {
         ESP_LOGI(_tag, "Failed to initialize Bluetooth controller!");
         assert(false);
@@ -81,6 +81,7 @@ void BluetoothDispatcher::_onAuthComplete(boolean success) const {
 void BluetoothDispatcher::_onDeviceConnected(BTAddress device) {
     ESP_LOGI(_tag, "New device connected: %s", device.toString(true).c_str());
     _buffer.clear();
+    _controller->flush();
     if (_deviceConnectedCallback) {
         _deviceConnectedCallback(device);
     }
@@ -92,12 +93,9 @@ void BluetoothDispatcher::onNewDeviceConnected(DeviceConnectedCb deviceConnected
 }
 
 void BluetoothDispatcher::sendPackage(const char *package) {
-    if (!_controller->connected())
+    if (!_controller->hasClient())
         return;
-    auto res = _controller->println(package);
+    _controller->println(package);
-    if (res == 0) {
-        delay(10);
-    }
 }
 
 

src/RF/BluetoothDispatcher.hpp

@@ -1,6 +1,8 @@
 // This is a personal academic project. Dear PVS-Studio, please check it.
 // PVS-Studio Static Code Analyzer for C, C++, C#, and Java: https://pvs-studio.com
 
+#define TX_QUEUE_SIZE 512
+
 #include "BluetoothSerial.h"
 #include "HardwareSerial.h"
 #include "esp_log.h"

src/Sensors/MPU.hpp

@@ -7,6 +7,7 @@
 #include "esp_log.h"
 
 static volatile bool _isDMPDataReady = false;
+
 static void IRAM_ATTR _dmpInterruption() {
     _isDMPDataReady = true;
 }
@@ -14,7 +15,7 @@ static void IRAM_ATTR _dmpInterruption() {
 typedef std::function<void()> OnCalibrationFinishedCb;
 
 class MPU {
-  public:
+public:
     MPU(MPU6050_6Axis_MotionApps20 *mpu) {
         assert(mpu != nullptr);
         _mpu = mpu;
@@ -39,7 +40,7 @@ class MPU {
             return false;
         }
         _mpu->setDLPFMode(MPU6050_DLPF_BW_10);  // 10 Hz bandwidth
-        //mpu.setFullScaleGyroRange(MPU6050_GYRO_FS_500); // set sensivity, not recomended
+        //mpu.setFullScaleGyroRange(MPU6050_GYRO_FS_500); // set sensivity, not recommended
         //mpu.setFullScaleAccelRange(MPU6050_ACCEL_FS_8);
 
         if (_mpu->dmpInitialize()) {
@@ -99,6 +100,11 @@ class MPU {
                 _prOffset[0] /= _calibrationsIterCounter;
                 _prOffset[1] /= _calibrationsIterCounter;
 
+                _prOffset[0] = DEG_TO_RAD * (180 - RAD_TO_DEG * _prOffset[0]);
+                _prOffset[1] = DEG_TO_RAD * (180 - RAD_TO_DEG * _prOffset[1]);
+                ESP_LOGI(_TAG, "Calibration offsets: roll: %f, pitch: %f", RAD_TO_DEG * _prOffset[1],
+                         RAD_TO_DEG * _prOffset[0]);
+
                 _isCalibration = false;
                 _updateOffsets();
                 _calibrationsIterCounter = 0;
@@ -113,9 +119,11 @@ class MPU {
                 _gxOffset += _gx;
                 _gyOffset += _gy;
                 _gzOffset += _gz;
-                _prOffset[0] += _ypr[1];
+                _prOffset[0] += pitch() * DEG_TO_RAD;
-                _prOffset[1] += _ypr[2];
+                _prOffset[1] += roll() * DEG_TO_RAD;
                 _calibrationsIterCounter++;
+                ESP_LOGI(_TAG, "Angles on calibration: roll: %f, pitch: %f", roll(),
+                         pitch());
             }
         }
 
@@ -126,21 +134,24 @@ class MPU {
     float accZInertial() const {
         return _AccZInertial;
     }
+
     float yaw() const {
-        return degrees(_ypr[0]);
+        return degrees(_ypr[0]) + 180.f;
     }
+
     float pitch() const {
         if (_isCalibration) {
-            return degrees(_ypr[1]);
+            return 360.f - (degrees(_ypr[1]) + 180);
         } else {
-            return degrees(_ypr[1] - _prOffset[0]);
+            return 360.f - (degrees(_ypr[1]) + 180) + degrees(_prOffset[0]);
         }
     }
+
     float roll() const {
         if (_isCalibration) {
-            return degrees(_ypr[2]);
+            return 360.f - (degrees(_ypr[2]) + 180);
         } else {
-            return degrees(_ypr[2] - _prOffset[1]);
+            return 360.f - (degrees(_ypr[2] + _prOffset[1]) + 180) + degrees(_prOffset[1]);
         }
     }
 
@@ -151,6 +162,7 @@ class MPU {
             return _ax - _axOffset;
         }
     }
+
     float ay() const {
         if (_isCalibration) {
             return _ay;
@@ -158,6 +170,7 @@ class MPU {
             return _ay - _ayOffset;
         }
     }
+
     float az() const {
         if (_isCalibration) {
             return _az;
@@ -177,6 +190,7 @@ class MPU {
             return _gx - _gxOffset;
         }
     }
+
     float gy() const {
         if (_isCalibration) {
             return _gy;
@@ -184,6 +198,7 @@ class MPU {
             return _gy - _gyOffset;
         }
     }
+
     float gz() const {
         if (_isCalibration) {
             return _gz;
@@ -200,9 +215,9 @@ class MPU {
         _calibrationFinishedCb = callback;
     }
 
-  private:
+private:
     MPU6050_6Axis_MotionApps20 *_mpu = nullptr;
-    float _ypr[3] = { 0 };
+    float _ypr[3] = {0};
     float _ax = 0, _ay = 0, _az = 0;
     float _gx = 0, _gy = 0, _gz = 0;
     float _gravity = 0;
@@ -210,9 +225,9 @@ class MPU {
 
     float _axOffset = 0, _ayOffset = 0, _azOffset = 0;
     float _gxOffset = 0, _gyOffset = 0, _gzOffset = 0;
-    float _prOffset[2] = { 0 };  // yaw isn't used
+    float _prOffset[2] = {0};  // yaw isn't used
 
-    uint8_t _fifoBuffer[45] = { 0 };
+    uint8_t _fifoBuffer[45] = {0};
     Preferences _preferences;
 
     const char *_TAG = "MPU6050 module";
@@ -239,8 +254,9 @@ class MPU {
         _gxOffset = _preferences.getFloat("gx", 0.f);
         _gyOffset = _preferences.getFloat("gy", 0.f);
         _gzOffset = _preferences.getFloat("gz", 0.f);
-        _ypr[1] = _preferences.getFloat("pitch", 0.f);
+        _prOffset[0] = _preferences.getFloat("pitch", 0.f);
-        _ypr[2] = _preferences.getFloat("roll", 0.f);
+        _prOffset[1] = _preferences.getFloat("roll", 0.f);
+        ESP_LOGI(_TAG, "Offsets loaded: pitch: %f, roll %f", _prOffset[0], _prOffset[1]);
     }
 
     void _updateOffsets() {
@@ -252,5 +268,6 @@ class MPU {
         _preferences.putFloat("gz", _gzOffset);
         _preferences.putFloat("pitch", _prOffset[0]);
         _preferences.putFloat("roll", _prOffset[1]);
+        ESP_LOGI(_TAG, "Offsets saved.");
     }
 };

src/Sensors/RangingSensor.cpp

@@ -20,7 +20,7 @@ RangingSensor::RangingSensor(TwoWire &i2c) {
 }
 
 void RangingSensor::startFlight() {
-    _start_distance = _distance;
+    //_start_distance = _distance;
 }
 
 void RangingSensor::tick() {

src/Sensors/RangingSensor.hpp

@@ -21,8 +21,8 @@ public:
 private:
     TwoWire *_i2c;
     VL53L0X _sensor;
-    uint32_t _distance; // [cm]
+    float _distance = 0; // [cm]
-    uint32_t _start_distance = 0; // [cm]
+    float _start_distance = 0; // [cm]
     static constexpr const char *_tag = "RangingSensor";
     constexpr static const int _MID_ARITM_WINDOW = 5;
 

src/Sensors/Sensors.cpp

@@ -54,6 +54,7 @@ void Sensors::measureBaseAltitudeSync(void) {
 
 void Sensors::measureBaseAltitudeAsync(void) {
     if (_heightSensor == HeightSensor::RangeMeterOnly) {
+        _ranging_sensor->startFlight();
         if (_onMeasuaringAltitudeFinished) {
             _onMeasuaringAltitudeFinished();
         }

src/main.cpp

@@ -11,7 +11,7 @@ static Application *app = nullptr;
 void setup() {
     Serial.begin(115200);
     app = new Application(new FlightDispatcher(
-            new BluetoothDispatcher(new BluetoothSerial()),
+            new BluetoothDispatcher(new BluetoothSerial(), "Redmi Note 8 Pro"),
             new FlightController(
                     new Sensors(new Barometer(new GyverBME280(i2c)),
                                 new RangingSensor(i2c),
@@ -25,7 +25,7 @@ void setup() {
                     new BrushedMotor(26, 27, 30000, 1, 10),
                     new PIDController(0.6f, 3.5f, 0.03f,
                                       new BrushedMotor(25, 33, 30000, 3, 10),
-                                      "RollControl"),  // pitch
+                                      "PitchControl"),  // pitch
                     true)));
 }