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

Code refactoring

Browse Source
This commit is contained in:
Goga Coder
2024-01-27 21:35:57 +07:00
parent 2aa9648dd4
commit fa0a664e43
14 changed files with 57 additions and 40 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
BoardI2C.hpp
View File

@@ -4,7 +4,7 @@
class BoardI2C : public TwoWire {
public:
BoardI2C(bool loop_on_fail = true) : TwoWire(0) {
BoardI2C(volatile bool loop_on_fail = true) : TwoWire(0) {
if (begin(I2C_SDA_PIN, I2C_SCL_PIN, 100000)) {
ESP_LOGI("I2CBus", "Bus initialized");
} else {

5
Logic/FlightController.cpp
View File

@@ -1,3 +1,6 @@
// 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
#include "FlightController.hpp"
FlightController::FlightController(Sensors *sensors, PIDController *pid1, PIDController *pid2,
@@ -186,6 +189,6 @@ PidSettings *FlightController::pidSettings() const {
PidSettings pid1 = { .p = _pid1->Kp, .i = _pid1->Ki, .d = _pid1->Kd };
PidSettings pid2 = { .p = _pid2->Kp, .i = _pid2->Ki, .d = _pid2->Kd };
PidSettings pid3 = { .p = _pid3->Kp, .i = _pid3->Ki, .d = _pid3->Kd };
PidSettings settings[] = { pid1, pid2, pid3 };
static PidSettings settings[] = { pid1, pid2, pid3 };
return settings;
}

1
Logic/FlightController.hpp
View File

@@ -1,4 +1,5 @@
#include "PID.hpp"
#include <memory>
#include "Sensors/Sensors.hpp"
enum DeviceStatus {

9
Logic/FlightDispatcher.cpp
View File

@@ -1,7 +1,10 @@
// 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
#include "FlightDispatcher.hpp"
FlightDispatcher::FlightDispatcher(BluetoothDispatcher *bluetoothDispatcher,
FlightController *flightController, bool loop_on_fail) {
FlightController *flightController, volatile bool loop_on_fail) {
assert(bluetoothDispatcher != nullptr);
assert(flightController != nullptr);
@@ -119,7 +122,9 @@ void FlightDispatcher::_onNewMessageReceived(char *package) {
void FlightDispatcher::_changeStatus(const DeviceStatus &newStatus) {
switch (newStatus) {
case DeviceStatus::IsImuCalibration: _flightController->startImuCalibration(); break;
case DeviceStatus::IsFlying: _flightController->startTakeoff(); break;
case DeviceStatus::IsFlying:
_changeStatus(DeviceStatus::IsPreparingForTakeoff);
break;
case DeviceStatus::IsPreparingForTakeoff: _flightController->startTakeoff(); break;
case DeviceStatus::IsBoarding: _flightController->startBoarding(); break;
default: break;

2
Logic/FlightDispatcher.hpp
View File

@@ -7,7 +7,7 @@ class FlightDispatcher {
/* Deserialize state and update it in FlightController. */
public:
FlightDispatcher(BluetoothDispatcher *bluetoothDispatcher,
FlightController *flightController, bool loop_on_fail = true);
FlightController *flightController, volatile bool loop_on_fail = true);
~FlightDispatcher();
void tick();
private:

1
Logic/PID.hpp
View File

@@ -11,6 +11,7 @@ class PIDController : public GyverPID {
assert(rotor != nullptr);
_rotor = rotor;
_rotorNumber = rotorNumber;
this->_dt = dt;
setLimits(0, _rotor->maxDuty());
_preferences.begin((String("r") + String(rotorNumber)).c_str());
_loadPreferences(p, i, d);

10
Motor/BrushedMotor.cpp
View File

@@ -1,7 +1,15 @@
// 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
#include "BrushedMotor.hpp"
BrushedMotor::BrushedMotor(uint32_t PwmAGpioNum, uint32_t PwmBGpioNum, uint32_t PwmFreqHz,
uint32_t McpwmResolutionHz, int McpwmGroupId) {}
uint32_t McpwmResolutionHz, int McpwmGroupId) {
_mcpwmResolutionHz = McpwmResolutionHz;
_pwmAGpioNum = McpwmGroupId;
_pwmBGpioNum = PwmBGpioNum;
_pwmFreqHz = PwmFreqHz;
}
void BrushedMotor::enable() {}

5
RF/BluetoothDispatcher.cpp
View File

@@ -1,3 +1,6 @@
// 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
#include "BluetoothDispatcher.hpp"
static DeviceConnectedCb deviceConnectedCallback = nullptr;
@@ -11,7 +14,7 @@ BluetoothDispatcher::BluetoothDispatcher(BluetoothSerial *controller, const char
_controller = controller;
}
bool BluetoothDispatcher::initialize(bool loop_on_fail, int readTimeoutMS) {
bool BluetoothDispatcher::initialize(volatile bool loop_on_fail, int readTimeoutMS) {
_controller->enableSSP();
_controller->onConfirmRequest([this](uint16_t pin) {
_onConfirmRequest(pin);

2
RF/BluetoothDispatcher.hpp
View File

@@ -25,7 +25,7 @@ typedef std::function<void(char *package)> NewPackageCallback;
class BluetoothDispatcher {
public:
BluetoothDispatcher(BluetoothSerial *controller, const char *device_name = "Helicopter");
bool initialize(bool loop_on_fail = true, int readTimeoutMS = 2);
bool initialize(volatile bool loop_on_fail = true, int readTimeoutMS = 2);
void tick();
void onNewPackageReceived(NewPackageCallback newPackageReceivedCb);
void onNewDeviceConnected(DeviceConnectedCb deviceConnectedCb);

2
Sensors/Barometer.hpp
View File

@@ -64,7 +64,7 @@ class Barometer {
GyverBME280 *_bme;
float _startedAltitude = 0;
bool _isCalibration = false;
uint32_t _calibrationTimer;
uint32_t _calibrationTimer = 0;
int _calibrationIterationsCounter = 0;
OnMeasuringFinishedCb _measuaringHeightFinishedCb = nullptr;
static constexpr int _calibrationIterationsCount = 1500;

12
Sensors/MPU.hpp
View File

@@ -196,17 +196,17 @@ class MPU {
private:
MPU6050_6Axis_MotionApps20 *_mpu = nullptr;
float _ypr[3];
float _ax, _ay, _az;
float _gx, _gy, _gz;
float _gravity;
float _ypr[3] = { 0 };
float _ax = 0, _ay = 0, _az = 0;
float _gx = 0, _gy = 0, _gz = 0;
float _gravity = 0;
float _AccZInertial = 0;
float _axOffset = 0, _ayOffset = 0, _azOffset = 0;
float _gxOffset = 0, _gyOffset = 0, _gzOffset = 0;
float _prOffset[2]; // yaw isn't used
float _prOffset[2] = { 0 }; // yaw isn't used
uint8_t _fifoBuffer[45];
uint8_t _fifoBuffer[45] = { 0 };
Preferences _preferences;
const char *_TAG = "MPU6050 module";

13
Sensors/Sensors.cpp
View File

@@ -1,6 +1,10 @@
// 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
#include "Sensors.hpp"
Sensors::Sensors(Barometer *barometer, MPU *mpu, Kalman2DFilter *filter, BatteryController *battery, bool loop_on_fail) {
Sensors::Sensors(Barometer *barometer, MPU *mpu, Kalman2DFilter *filter,
BatteryController *battery, volatile bool loop_on_fail) {
assert(barometer != nullptr);
assert(mpu != nullptr);
assert(filter != nullptr);
@@ -56,7 +60,7 @@ float Sensors::rawFlightHeight(void) const {
float Sensors::flightHeight(void) const {
/* Returns flight height from cache immediatly */
if (_flightHeightFromBarometer == NAN) {
if (_flightHeightFromBarometer == 0.f) {
ESP_LOGW(_tag, "flightHeight called, but tick() has called never");
return rawFlightHeight();
} else {
@@ -65,7 +69,7 @@ float Sensors::flightHeight(void) const {
}
MpuData Sensors::mpuData() const {
if (_mpuData.ax == NAN and _mpuData.gravity == NAN) {
if (_mpuData.ax == 0.f and _mpuData.gravity == 0.f) {
ESP_LOGW(_tag, "MPU data looks like .tick() has never called");
}
return _mpuData;
@@ -78,7 +82,8 @@ bool Sensors::tick(void) {
bool err;
bool isMpuDataReady = _mpu->tick(err);
if (isMpuDataReady and !err) {
_2d_filter->filter(_mpu->accZInertial(), _barometer->flightHeight(), _flightHeightFromBarometer, _zVelocityAltitude);
_2d_filter->filter(_mpu->accZInertial(), _barometer->flightHeight(),
_flightHeightFromBarometer, _zVelocityAltitude);
_mpuData = { .ax = _mpu->ax(),
.ay = _mpu->ay(),
.az = _mpu->az(),

9
Sensors/Sensors.hpp
View File

@@ -16,7 +16,8 @@ struct MpuData {
class Sensors {
public:
Sensors(Barometer *barometer, MPU *mpu, Kalman2DFilter *filter, BatteryController *battery, bool loop_on_fail = true);
Sensors(Barometer *barometer, MPU *mpu, Kalman2DFilter *filter,
BatteryController *battery, bool loop_on_fail = true);
~Sensors();
@@ -42,9 +43,9 @@ class Sensors {
Kalman2DFilter *_2d_filter = nullptr;
/* cached filtered values */
float _flightHeightFromBarometer = NAN;
float _zVelocityAltitude = NAN;
MpuData _mpuData = { NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN };
float _flightHeightFromBarometer = 0.f;
float _zVelocityAltitude = 0.f;
MpuData _mpuData = { 0.f };
static constexpr const char *_tag = "Sensors";

24
main.cpp
View File

@@ -1,12 +1,12 @@
// 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
#include "App.hpp"
#include "BoardI2C.hpp"
//#include "MPU6050_6Axis_MotionApps20.h"
#define IGNORE_CHARGE true
BoardI2C i2c;
/*
BluetoothDispatcher bluetoothDispatcher(new BluetoothSerial());
Sensors *sensors = nullptr; */
static Application *app = nullptr;
@@ -17,21 +17,11 @@ void setup() {
new FlightController(
new Sensors(new Barometer(new GyverBME280(i2c)), new MPU(new MPU6050(MPU6050_DEFAULT_ADDRESS, &i2c)),
new Kalman2DFilter(10.f, 1.f, 1.8f), new BatteryController()),
new PIDController(1, 1, 1, new BrushedMotor(1, 2, 3, 4, 1), 1),
new PIDController(1, 1, 1, new BrushedMotor(1, 2, 3, 4, 2), 2),
new PIDController(1, 1, 1, new BrushedMotor(1, 2, 3, 4, 3), 3), IGNORE_CHARGE)));
/*sensors = new Sensors(
new Barometer(new GyverBME280(i2c)), new MPU(new
MPU6050(MPU6050_DEFAULT_ADDRESS, &i2c)), new Kalman2DFilter(10.f, 1.f, 1.8f),
new BatteryController()); sensors->measureBaseAltitudeSync();
bluetoothDispatcher.initialize();
Serial.println("System initialized");*/
new PIDController(1.f, 1.f, 1.f, new BrushedMotor(1, 2, 3, 4, 1), 1),
new PIDController(1.f, 1.f, 1.f, new BrushedMotor(1, 2, 3, 4, 2), 2),
new PIDController(1.f, 1.f, 1.f, new BrushedMotor(1, 2, 3, 4, 3), 3), IGNORE_CHARGE)));
}
void loop() {
app->tick();
/*
sensors->tick();
bluetoothDispatcher.tick();
delay(1);*/
}