refactor: #pragma once
This commit is contained in:
@@ -1,6 +1,8 @@
|
|||||||
// This is a personal academic project. Dear PVS-Studio, please check it.
|
// 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
|
// PVS-Studio Static Code Analyzer for C, C++, C#, and Java: https://pvs-studio.com
|
||||||
|
|
||||||
|
#pragma once
|
||||||
|
|
||||||
#define CONFIG_DISABLE_HAL_LOCKS 0
|
#define CONFIG_DISABLE_HAL_LOCKS 0
|
||||||
|
|
||||||
#include "Wire.h"
|
#include "Wire.h"
|
||||||
@@ -18,4 +20,4 @@ public:
|
|||||||
assert(false);
|
assert(false);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
};
|
};
|
||||||
|
|||||||
@@ -1,37 +1,38 @@
|
|||||||
// This is a personal academic project. Dear PVS-Studio, please check it.
|
// 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
|
// PVS-Studio Static Code Analyzer for C, C++, C#, and Java: https://pvs-studio.com
|
||||||
|
#pragma once
|
||||||
|
|
||||||
#include <BasicLinearAlgebra.h>
|
#include <BasicLinearAlgebra.h>
|
||||||
|
|
||||||
class Kalman2DFilter {
|
class Kalman2DFilter {
|
||||||
public:
|
public:
|
||||||
Kalman2DFilter(float dt = 4.f, float accelUncertainty = 10.f, float barometerUncertainty = 100.f) {
|
Kalman2DFilter(float dt = 4.f, float accelUncertainty = 10.f, float barometerUncertainty = 100.f) {
|
||||||
dt /= 1000.f;
|
dt /= 1000.f;
|
||||||
F = { 1, dt, 0, 1 };
|
F = {1, dt, 0, 1};
|
||||||
G = { 0.5f * dt * dt, dt };
|
G = {0.5f * dt * dt, dt};
|
||||||
H = { 1, 0 };
|
H = {1, 0};
|
||||||
I = { 1, 0, 0, 1 };
|
I = {1, 0, 0, 1};
|
||||||
Q = G * ~G * accelUncertainty * accelUncertainty;
|
Q = G * ~G * accelUncertainty * accelUncertainty;
|
||||||
R = { barometerUncertainty * barometerUncertainty };
|
R = {barometerUncertainty * barometerUncertainty};
|
||||||
P = { 0, 0, 0, 0 };
|
P = {0, 0, 0, 0};
|
||||||
S = { 0, 0 };
|
S = {0, 0};
|
||||||
}
|
}
|
||||||
|
|
||||||
void filter(const float &AccZInertial, const float &AltitudeBarometer,
|
void filter(const float &AccZInertial, const float &AltitudeBarometer,
|
||||||
float &AltitudeKalman, float &VelocityVerticalKalman) {
|
float &AltitudeKalman, float &VelocityVerticalKalman) {
|
||||||
Acc = { AccZInertial };
|
Acc = {AccZInertial};
|
||||||
S = F * S + G * Acc;
|
S = F * S + G * Acc;
|
||||||
P = F * P * ~F + Q;
|
P = F * P * ~F + Q;
|
||||||
L = H * P * ~H + R;
|
L = H * P * ~H + R;
|
||||||
K = P * ~H * Inverse(L);
|
K = P * ~H * Inverse(L);
|
||||||
M = { AltitudeBarometer };
|
M = {AltitudeBarometer};
|
||||||
S = S + K * (M - H * S);
|
S = S + K * (M - H * S);
|
||||||
AltitudeKalman = S(0, 0);
|
AltitudeKalman = S(0, 0);
|
||||||
VelocityVerticalKalman = S(1, 0);
|
VelocityVerticalKalman = S(1, 0);
|
||||||
P = (I - K * H) * P;
|
P = (I - K * H) * P;
|
||||||
}
|
}
|
||||||
|
|
||||||
private:
|
private:
|
||||||
BLA::Matrix<2, 2> F;
|
BLA::Matrix<2, 2> F;
|
||||||
BLA::Matrix<2, 1> G;
|
BLA::Matrix<2, 1> G;
|
||||||
BLA::Matrix<2, 2> P;
|
BLA::Matrix<2, 2> P;
|
||||||
@@ -44,4 +45,4 @@ class Kalman2DFilter {
|
|||||||
BLA::Matrix<1, 1> R;
|
BLA::Matrix<1, 1> R;
|
||||||
BLA::Matrix<1, 1> L;
|
BLA::Matrix<1, 1> L;
|
||||||
BLA::Matrix<1, 1> M;
|
BLA::Matrix<1, 1> M;
|
||||||
};
|
};
|
||||||
|
|||||||
@@ -1,4 +1,5 @@
|
|||||||
// упрощённый Калман для одномерного случая
|
// упрощённый Калман для одномерного случая
|
||||||
|
#pragma once
|
||||||
|
|
||||||
#ifndef _GKalman_h
|
#ifndef _GKalman_h
|
||||||
#define _GKalman_h
|
#define _GKalman_h
|
||||||
|
|||||||
@@ -1,4 +1,5 @@
|
|||||||
// быстрый медианный фильтр 3-го порядка
|
// быстрый медианный фильтр 3-го порядка
|
||||||
|
#pragma once
|
||||||
|
|
||||||
#ifndef _GMedian3_h
|
#ifndef _GMedian3_h
|
||||||
#define _GMedian3_h
|
#define _GMedian3_h
|
||||||
|
|||||||
@@ -1,4 +1,5 @@
|
|||||||
// экспоненциальное бегущее среднее
|
// экспоненциальное бегущее среднее
|
||||||
|
#pragma once
|
||||||
|
|
||||||
#ifndef _GFilterRA_h
|
#ifndef _GFilterRA_h
|
||||||
#define _GFilterRA_h
|
#define _GFilterRA_h
|
||||||
|
|||||||
@@ -1,5 +1,6 @@
|
|||||||
// This is a personal academic project. Dear PVS-Studio, please check it.
|
// 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
|
// PVS-Studio Static Code Analyzer for C, C++, C#, and Java: https://pvs-studio.com
|
||||||
|
#pragma once
|
||||||
|
|
||||||
#include "PIDController.hpp"
|
#include "PIDController.hpp"
|
||||||
#include "Sensors/Sensors.hpp"
|
#include "Sensors/Sensors.hpp"
|
||||||
|
|||||||
@@ -1,5 +1,6 @@
|
|||||||
// This is a personal academic project. Dear PVS-Studio, please check it.
|
// 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
|
// PVS-Studio Static Code Analyzer for C, C++, C#, and Java: https://pvs-studio.com
|
||||||
|
#pragma once
|
||||||
|
|
||||||
#include "FlightController.hpp"
|
#include "FlightController.hpp"
|
||||||
#include "GSON.h"
|
#include "GSON.h"
|
||||||
@@ -43,4 +44,4 @@ private:
|
|||||||
uint32_t _telemetryTimer = millis();
|
uint32_t _telemetryTimer = millis();
|
||||||
BluetoothDispatcher *_bluetoothDispatcher;
|
BluetoothDispatcher *_bluetoothDispatcher;
|
||||||
FlightController *_flightController;
|
FlightController *_flightController;
|
||||||
};
|
};
|
||||||
|
|||||||
@@ -1,6 +1,8 @@
|
|||||||
// This is a personal academic project. Dear PVS-Studio, please check it.
|
// 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
|
// PVS-Studio Static Code Analyzer for C, C++, C#, and Java: https://pvs-studio.com
|
||||||
|
|
||||||
|
#pragma once
|
||||||
|
|
||||||
#include "Motor/BrushedMotor.hpp"
|
#include "Motor/BrushedMotor.hpp"
|
||||||
#include "SavedPidRegulator.hpp"
|
#include "SavedPidRegulator.hpp"
|
||||||
#include "esp_log.h"
|
#include "esp_log.h"
|
||||||
@@ -10,7 +12,7 @@ class PIDController : public SavedPidRegulator {
|
|||||||
public:
|
public:
|
||||||
PIDController(float p, float i, float d, BrushedMotor *rotor,
|
PIDController(float p, float i, float d, BrushedMotor *rotor,
|
||||||
const char *name = "PID", uint16_t dt = 10, const bool isEnabled = false)
|
const char *name = "PID", uint16_t dt = 10, const bool isEnabled = false)
|
||||||
: SavedPidRegulator(p, i, d, name, dt) {
|
: SavedPidRegulator(p, i, d, name, dt) {
|
||||||
assert(rotor != nullptr);
|
assert(rotor != nullptr);
|
||||||
_rotor = rotor;
|
_rotor = rotor;
|
||||||
_isEnabled = isEnabled;
|
_isEnabled = isEnabled;
|
||||||
@@ -50,4 +52,4 @@ private:
|
|||||||
BrushedMotor *_rotor = nullptr;
|
BrushedMotor *_rotor = nullptr;
|
||||||
uint32_t _pid_timer = 0;
|
uint32_t _pid_timer = 0;
|
||||||
bool _isEnabled = false;
|
bool _isEnabled = false;
|
||||||
};
|
};
|
||||||
|
|||||||
@@ -1,3 +1,4 @@
|
|||||||
|
#pragma once
|
||||||
#include "GyverPID.h"
|
#include "GyverPID.h"
|
||||||
#include "Preferences.h"
|
#include "Preferences.h"
|
||||||
#include "esp_log.h"
|
#include "esp_log.h"
|
||||||
|
|||||||
@@ -72,6 +72,6 @@ void BrushedMotor::reverse() {
|
|||||||
_apply_directional();
|
_apply_directional();
|
||||||
}
|
}
|
||||||
|
|
||||||
void BrushedMotor::coast() {
|
void BrushedMotor::coast() const {
|
||||||
setDuty(0);
|
setDuty(0);
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -1,10 +1,10 @@
|
|||||||
// This is a personal academic project. Dear PVS-Studio, please check it.
|
// 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
|
// PVS-Studio Static Code Analyzer for C, C++, C#, and Java: https://pvs-studio.com
|
||||||
|
#pragma once
|
||||||
|
|
||||||
#include <cstdint>
|
#include <cstdint>
|
||||||
#include "Arduino.h"
|
#include "Arduino.h"
|
||||||
#include "string.h"
|
#include <cstring>
|
||||||
#include "driver/ledc.h"
|
|
||||||
|
|
||||||
|
|
||||||
class BrushedMotor {
|
class BrushedMotor {
|
||||||
@@ -25,7 +25,7 @@ public:
|
|||||||
|
|
||||||
void reverse();
|
void reverse();
|
||||||
|
|
||||||
void coast();
|
void coast() const;
|
||||||
|
|
||||||
void decrement(uint32_t k) const;
|
void decrement(uint32_t k) const;
|
||||||
|
|
||||||
@@ -40,4 +40,4 @@ private:
|
|||||||
String _tag_name = "BrushedMotor";
|
String _tag_name = "BrushedMotor";
|
||||||
|
|
||||||
void _apply_directional() const;
|
void _apply_directional() const;
|
||||||
};
|
};
|
||||||
|
|||||||
@@ -1,5 +1,6 @@
|
|||||||
// This is a personal academic project. Dear PVS-Studio, please check it.
|
// 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
|
// PVS-Studio Static Code Analyzer for C, C++, C#, and Java: https://pvs-studio.com
|
||||||
|
#pragma once
|
||||||
|
|
||||||
#define TX_QUEUE_SIZE 512
|
#define TX_QUEUE_SIZE 512
|
||||||
|
|
||||||
@@ -58,4 +59,4 @@ private:
|
|||||||
|
|
||||||
static constexpr uint16_t _buffer_size = 512;
|
static constexpr uint16_t _buffer_size = 512;
|
||||||
mString<_buffer_size> _buffer;
|
mString<_buffer_size> _buffer;
|
||||||
};
|
};
|
||||||
|
|||||||
@@ -1,5 +1,6 @@
|
|||||||
// This is a personal academic project. Dear PVS-Studio, please check it.
|
// 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
|
// PVS-Studio Static Code Analyzer for C, C++, C#, and Java: https://pvs-studio.com
|
||||||
|
#pragma once
|
||||||
|
|
||||||
#include "GyverBME280.h"
|
#include "GyverBME280.h"
|
||||||
|
|
||||||
|
|||||||
@@ -1,5 +1,6 @@
|
|||||||
// This is a personal academic project. Dear PVS-Studio, please check it.
|
// 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
|
// PVS-Studio Static Code Analyzer for C, C++, C#, and Java: https://pvs-studio.com
|
||||||
|
#pragma once
|
||||||
|
|
||||||
#include "GyverBME280.h"
|
#include "GyverBME280.h"
|
||||||
|
|
||||||
|
|||||||
@@ -1,5 +1,6 @@
|
|||||||
// This is a personal academic project. Dear PVS-Studio, please check it.
|
// 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
|
// PVS-Studio Static Code Analyzer for C, C++, C#, and Java: https://pvs-studio.com
|
||||||
|
#pragma once
|
||||||
|
|
||||||
#include "board_pins.h"
|
#include "board_pins.h"
|
||||||
|
|
||||||
@@ -36,4 +37,4 @@ private:
|
|||||||
static float _map(float x, float in_min, float in_max, float out_min, float out_max) {
|
static float _map(float x, float in_min, float in_max, float out_min, float out_max) {
|
||||||
return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
|
return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
|
||||||
}
|
}
|
||||||
};
|
};
|
||||||
|
|||||||
@@ -1,5 +1,6 @@
|
|||||||
// This is a personal academic project. Dear PVS-Studio, please check it.
|
// 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
|
// PVS-Studio Static Code Analyzer for C, C++, C#, and Java: https://pvs-studio.com
|
||||||
|
#pragma once
|
||||||
|
|
||||||
#include "MPU6050_6Axis_MotionApps20.h"
|
#include "MPU6050_6Axis_MotionApps20.h"
|
||||||
#include "Preferences.h"
|
#include "Preferences.h"
|
||||||
@@ -39,7 +40,7 @@ public:
|
|||||||
ESP_LOGE(_TAG, "MPU6050 test connection failed!");
|
ESP_LOGE(_TAG, "MPU6050 test connection failed!");
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
_mpu->setDLPFMode(MPU6050_DLPF_BW_10); // 10 Hz bandwidth
|
_mpu->setDLPFMode(MPU6050_DLPF_BW_10); // 10 Hz bandwidth
|
||||||
//mpu.setFullScaleGyroRange(MPU6050_GYRO_FS_500); // set sensivity, not recommended
|
//mpu.setFullScaleGyroRange(MPU6050_GYRO_FS_500); // set sensivity, not recommended
|
||||||
//mpu.setFullScaleAccelRange(MPU6050_ACCEL_FS_8);
|
//mpu.setFullScaleAccelRange(MPU6050_ACCEL_FS_8);
|
||||||
|
|
||||||
@@ -225,7 +226,7 @@ private:
|
|||||||
|
|
||||||
float _axOffset = 0, _ayOffset = 0, _azOffset = 0;
|
float _axOffset = 0, _ayOffset = 0, _azOffset = 0;
|
||||||
float _gxOffset = 0, _gyOffset = 0, _gzOffset = 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;
|
Preferences _preferences;
|
||||||
|
|||||||
@@ -1,14 +1,13 @@
|
|||||||
//
|
|
||||||
// Created by gogacoder on 10.03.24.
|
|
||||||
//
|
|
||||||
|
|
||||||
#include "RangingSensor.hpp"
|
#include "RangingSensor.hpp"
|
||||||
#include "esp_log.h"
|
#include "esp_log.h"
|
||||||
|
|
||||||
RangingSensor::RangingSensor() {}
|
RangingSensor::RangingSensor() {
|
||||||
|
}
|
||||||
|
|
||||||
void RangingSensor::startFlight() {}
|
void RangingSensor::startFlight() {
|
||||||
|
}
|
||||||
|
|
||||||
void RangingSensor::tick() {}
|
void RangingSensor::tick() {
|
||||||
|
}
|
||||||
|
|
||||||
uint32_t RangingSensor::getDistance() { return 0; }
|
uint32_t RangingSensor::getDistance() { return 0; }
|
||||||
|
|||||||
@@ -1,6 +1,6 @@
|
|||||||
//
|
// This is a personal academic project. Dear PVS-Studio, please check it.
|
||||||
// Created by gogacoder on 10.03.24.
|
// PVS-Studio Static Code Analyzer for C, C++, C#, and Java: https://pvs-studio.com
|
||||||
//
|
#pragma once
|
||||||
|
|
||||||
#ifndef HELICOPTER_FIRMWARE_RANGINGSENSOR_H
|
#ifndef HELICOPTER_FIRMWARE_RANGINGSENSOR_H
|
||||||
#define HELICOPTER_FIRMWARE_RANGINGSENSOR_H
|
#define HELICOPTER_FIRMWARE_RANGINGSENSOR_H
|
||||||
|
|||||||
@@ -1,6 +1,7 @@
|
|||||||
// This is a personal academic project. Dear PVS-Studio, please check it.
|
// 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
|
// PVS-Studio Static Code Analyzer for C, C++, C#, and Java: https://pvs-studio.com
|
||||||
|
|
||||||
|
#pragma once
|
||||||
#include "Barometer.hpp"
|
#include "Barometer.hpp"
|
||||||
#include "BatteryController.hpp"
|
#include "BatteryController.hpp"
|
||||||
#include "Filters/Kalman2DFilter.hpp"
|
#include "Filters/Kalman2DFilter.hpp"
|
||||||
@@ -47,7 +48,7 @@ public:
|
|||||||
|
|
||||||
MpuData mpuData(void) const;
|
MpuData mpuData(void) const;
|
||||||
|
|
||||||
int batteryCharge(void) const; // [%]
|
int batteryCharge(void) const; // [%]
|
||||||
|
|
||||||
bool tick(void);
|
bool tick(void);
|
||||||
|
|
||||||
@@ -67,4 +68,4 @@ private:
|
|||||||
|
|
||||||
|
|
||||||
static constexpr const char *_tag = "Sensors";
|
static constexpr const char *_tag = "Sensors";
|
||||||
};
|
};
|
||||||
|
|||||||
@@ -1,9 +1,10 @@
|
|||||||
// This is a personal academic project. Dear PVS-Studio, please check it.
|
// 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
|
// PVS-Studio Static Code Analyzer for C, C++, C#, and Java: https://pvs-studio.com
|
||||||
|
|
||||||
|
#pragma once
|
||||||
#define I2C_SDA_PIN 21
|
#define I2C_SDA_PIN 21
|
||||||
#define I2C_SCL_PIN 18
|
#define I2C_SCL_PIN 18
|
||||||
#define MPU6050_INT_PIN 19
|
#define MPU6050_INT_PIN 19
|
||||||
#define BATTERY_ADC_CHANNEL ADC2_CHANNEL_0
|
#define BATTERY_ADC_CHANNEL ADC2_CHANNEL_0
|
||||||
#define BATTERY_DATA_PIN 4
|
#define BATTERY_DATA_PIN 4
|
||||||
#define BATTERY_DATA_SWITCH_PIN 17
|
#define BATTERY_DATA_SWITCH_PIN 17
|
||||||
|
|||||||
Reference in New Issue
Block a user