Helicopter/firmware
2
1
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Error fixes and base flight logic

Browse Source
This commit is contained in:
Goga Coder
2024-04-03 20:38:44 +07:00
parent 47f91eeae5
commit 0cb666c860
13 changed files with 113 additions and 61 deletions
Download Patch File Download Diff File Expand all files Collapse all files

32
src/Logic/FlightController.cpp
View File

@@ -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
_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);
@@ -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());
}

21
src/Logic/FlightController.hpp
View File

@@ -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
};

22
src/Logic/FlightDispatcher.cpp
View File

@@ -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();

17
src/Logic/PIDController.hpp
View File

@@ -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;

4
src/Motor/BrushedMotor.cpp
View File

@@ -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();

2
src/Motor/BrushedMotor.hpp
View File

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

12
src/RF/BluetoothDispatcher.cpp
View File

@@ -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);
}

2
src/RF/BluetoothDispatcher.hpp
View File

@@ -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"

47
src/Sensors/MPU.hpp
View File

@@ -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[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.");
}
};

2
src/Sensors/RangingSensor.cpp
View File

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

4
src/Sensors/RangingSensor.hpp
View File

@@ -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;

1
src/Sensors/Sensors.cpp
View File

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

4
src/main.cpp
View File

@@ -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)));
}