From 0cb666c8602057d68e91e007e2622dc766a96ec9 Mon Sep 17 00:00:00 2001 From: gogacoder Date: Wed, 3 Apr 2024 20:38:44 +0700 Subject: [PATCH] Error fixes and base flight logic --- src/Logic/FlightController.cpp | 36 ++++++++++++++++++-------- src/Logic/FlightController.hpp | 21 +++++++++++---- src/Logic/FlightDispatcher.cpp | 22 ++++++++-------- src/Logic/PIDController.hpp | 17 +++++++----- src/Motor/BrushedMotor.cpp | 4 +++ src/Motor/BrushedMotor.hpp | 2 ++ src/RF/BluetoothDispatcher.cpp | 12 ++++----- src/RF/BluetoothDispatcher.hpp | 2 ++ src/Sensors/MPU.hpp | 47 +++++++++++++++++++++++----------- src/Sensors/RangingSensor.cpp | 2 +- src/Sensors/RangingSensor.hpp | 4 +-- src/Sensors/Sensors.cpp | 1 + src/main.cpp | 4 +-- 13 files changed, 113 insertions(+), 61 deletions(-) diff --git a/src/Logic/FlightController.cpp b/src/Logic/FlightController.cpp index d972e5c..2835b9f 100644 --- a/src/Logic/FlightController.cpp +++ b/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(); - } + _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 */ - _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 - _status = DeviceStatus::Idle; - _tailRotorController->disable(); + stopAllRotors(); } 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); @@ -197,4 +205,10 @@ PidSettings FlightController::pidSettings() const { settings.yawController = {.p = _yawController->Kp, .i = _yawController->Ki, .d = _yawController->Kd}; 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()); } \ No newline at end of file diff --git a/src/Logic/FlightController.hpp b/src/Logic/FlightController.hpp index 95b4ec2..86a42c5 100644 --- a/src/Logic/FlightController.hpp +++ b/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 _defaultHorizontAngle = 90.f; // [degrees] - static constexpr float _defaultStopMotorHeight = 5; // [cm] + static constexpr float _defaultFlightHeight = 55.f; // [cm] + static constexpr float _defaultHorizontAngle = 180.f; // [degrees] + static constexpr float _boarding_speed = 0.1; // [cm/10 ms] + static constexpr float _defaultStopMotorHeight = 15; // [cm] + bool _unlimitedBattery = false; // for debug purposes }; \ No newline at end of file diff --git a/src/Logic/FlightDispatcher.cpp b/src/Logic/FlightDispatcher.cpp index 81dd715..ee2a897 100644 --- a/src/Logic/FlightDispatcher.cpp +++ b/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["i"] = settings.flightController.i; - pkg["d"] = settings.flightController.d; + pkg.addFloat("p", settings.flightController.p, 5); + pkg.addFloat("i", settings.flightController.i, 5); + pkg.addFloat("d", settings.flightController.d, 5); pkg.endObj(); pkg.beginObj("p2"); - pkg["p"] = settings.yawController.p; - pkg["i"] = settings.yawController.i; - pkg["d"] = settings.yawController.d; + pkg.addFloat("p", settings.pitchController.p, 5); + pkg.addFloat("i", settings.pitchController.i, 5); + pkg.addFloat("d", settings.pitchController.d, 5); pkg.endObj(); pkg.beginObj("p3"); - pkg["p"] = settings.pitchController.p; - pkg["i"] = settings.pitchController.i; - pkg["d"] = settings.pitchController.d; + pkg.addFloat("p", settings.yawController.p, 5); + pkg.addFloat("i", settings.yawController.i, 5); + pkg.addFloat("d", settings.yawController.d, 5); pkg.endObj(); pkg.endObj(); diff --git a/src/Logic/PIDController.hpp b/src/Logic/PIDController.hpp index 3e93809..88a3179 100644 --- a/src/Logic/PIDController.hpp +++ b/src/Logic/PIDController.hpp @@ -14,8 +14,8 @@ public: assert(rotor != nullptr); _rotor = rotor; _isEnabled = isEnabled; - this->_dt = dt; - setLimits(0, _rotor->maxDuty()); + setLimits(-_rotor->maxDuty(), _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() { - if (millis() - _pid_timer >= _dt and _isEnabled) { - _rotor->setDuty((uint16_t) getResultTimer()); - _pid_timer = millis(); + void update() { + if (_isEnabled) { + //ESP_LOGI(_tag, "Setpoint: %f", setpoint); + //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; diff --git a/src/Motor/BrushedMotor.cpp b/src/Motor/BrushedMotor.cpp index d0f929a..3775ef6 100644 --- a/src/Motor/BrushedMotor.cpp +++ b/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(); diff --git a/src/Motor/BrushedMotor.hpp b/src/Motor/BrushedMotor.hpp index 0de722a..95a3f82 100644 --- a/src/Motor/BrushedMotor.hpp +++ b/src/Motor/BrushedMotor.hpp @@ -27,6 +27,8 @@ public: void coast(); + void decrement(uint32_t k) const; + private: uint8_t _channel; uint32_t _frequency; diff --git a/src/RF/BluetoothDispatcher.cpp b/src/RF/BluetoothDispatcher.cpp index 1e2f388..df8f8c2 100644 --- a/src/RF/BluetoothDispatcher.cpp +++ b/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); - bool success = _controller->begin(_device_name, false); + //_controller->setTimeout(readTimeoutMS); 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); - if (res == 0) { - delay(10); - } + _controller->println(package); } diff --git a/src/RF/BluetoothDispatcher.hpp b/src/RF/BluetoothDispatcher.hpp index bac8274..921f268 100644 --- a/src/RF/BluetoothDispatcher.hpp +++ b/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" diff --git a/src/Sensors/MPU.hpp b/src/Sensors/MPU.hpp index a3d7231..52869b6 100644 --- a/src/Sensors/MPU.hpp +++ b/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 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[1] += _ypr[2]; + _prOffset[0] += pitch() * DEG_TO_RAD; + _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); - _ypr[2] = _preferences.getFloat("roll", 0.f); + _prOffset[0] = _preferences.getFloat("pitch", 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."); } }; diff --git a/src/Sensors/RangingSensor.cpp b/src/Sensors/RangingSensor.cpp index 241b6b5..3039e0d 100644 --- a/src/Sensors/RangingSensor.cpp +++ b/src/Sensors/RangingSensor.cpp @@ -20,7 +20,7 @@ RangingSensor::RangingSensor(TwoWire &i2c) { } void RangingSensor::startFlight() { - _start_distance = _distance; + //_start_distance = _distance; } void RangingSensor::tick() { diff --git a/src/Sensors/RangingSensor.hpp b/src/Sensors/RangingSensor.hpp index 1b5c54f..c86fddc 100644 --- a/src/Sensors/RangingSensor.hpp +++ b/src/Sensors/RangingSensor.hpp @@ -21,8 +21,8 @@ public: private: TwoWire *_i2c; VL53L0X _sensor; - uint32_t _distance; // [cm] - uint32_t _start_distance = 0; // [cm] + float _distance = 0; // [cm] + float _start_distance = 0; // [cm] static constexpr const char *_tag = "RangingSensor"; constexpr static const int _MID_ARITM_WINDOW = 5; diff --git a/src/Sensors/Sensors.cpp b/src/Sensors/Sensors.cpp index ad4d571..e395255 100644 --- a/src/Sensors/Sensors.cpp +++ b/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(); } diff --git a/src/main.cpp b/src/main.cpp index 7510c1e..200baa0 100644 --- a/src/main.cpp +++ b/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))); }