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

Fast flight preparing and reboot on failed initialization

Browse Source
This commit is contained in:
Goga Coder
2024-03-13 22:37:48 +07:00
parent e27b3ca794
commit 839dbee16e
11 changed files with 27 additions and 22 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
platformio.ini
View File

@@ -41,7 +41,7 @@ build_src_flags =
-Wno-error=pedantic
-Wpedantic
-O2
build_flags = -DCORE_DEBUG_LEVEL=4
build_flags = -DCORE_DEBUG_LEVEL=3
check_tool = pvs-studio
check_flags = pvs-studio: --analysis-mode=4 --exclude-path=.pio/libdeps --lic-file=~/.config/PVS-Studio/PVS-Studio.lic
extra_scripts = post:convert_sysincludes.py

4
src/BoardI2C.hpp
View File

@@ -9,13 +9,13 @@
class BoardI2C : public TwoWire {
public:
BoardI2C(volatile bool loop_on_fail = true) : TwoWire(0) {
BoardI2C() : TwoWire(0) {
if (begin(I2C_SDA_PIN, I2C_SCL_PIN, 100000)) {
setTimeOut(10);
ESP_LOGI("I2CBus", "Bus initialized");
} else {
ESP_LOGE("I2CBus", "Failed to initialize the i2c software bus!");
while (loop_on_fail);
assert(false);
}
}
};

4
src/Logic/FlightController.cpp
View File

@@ -181,13 +181,13 @@ void FlightController::setHeightControllerParams(float p, float i, float d) {
void FlightController::setYawControllerParams(float p, float i, float d) {
if (_status == DeviceStatus::Idle) {
_heightController->setParams({.p = p, .i = i, .d = d});
_yawController->setParams({.p = p, .i = i, .d = d});
}
}
void FlightController::setPitchControllerParams(float p, float i, float d) {
if (_status == DeviceStatus::Idle) {
_heightController->setParams({.p = p, .i = i, .d = d});
_tailRotorController->setParams({.p = p, .i = i, .d = d});
}
}

4
src/Logic/FlightDispatcher.cpp
View File

@@ -47,7 +47,7 @@ void FlightDispatcher::_onNewDeviceConnected(BTAddress device) {
void FlightDispatcher::_onNewMessageReceived(char *package) {
using sutil::SH;
ESP_LOGI(_tag, "Received new package: %s", package);
ESP_LOGD(_tag, "Received new package: %s", package);
gson::Doc pkg;
if (!pkg.parse(package, _jsonMaxDepth)) {
ESP_LOGE(_tag, "Parcing error occured with new package (error %s, place %d): %s",
@@ -57,10 +57,8 @@ void FlightDispatcher::_onNewMessageReceived(char *package) {
for (int keyIndex = 0; keyIndex < pkg.length(); ++keyIndex) {
auto pair = pkg[pkg.keyHash(keyIndex)];
auto value = pair.value();
ESP_LOGD(_tag, "Key: %zu", pkg.keyHash(keyIndex));
if (!pair.valid()) {
ESP_LOGW(_tag, "Pair isn't valid and was skipped");
continue;
}

5
src/RF/BluetoothDispatcher.hpp
View File

@@ -29,7 +29,7 @@ class BluetoothDispatcher {
public:
BluetoothDispatcher(BluetoothSerial *controller, const char *device_name = "Helicopter");
bool initialize(volatile bool loop_on_fail = true, int readTimeoutMS = 10);
bool initialize(int readTimeoutMS = 10);
void tick(const char message_delimeter = '\n');
@@ -54,7 +54,6 @@ private:
NewPackageCallback _newPackageReceivedCb = nullptr;
constexpr static const char *_tag = "BluetoothDispatcher";
static constexpr uint16_t _buffer_size = 522;
static constexpr uint16_t _real_buffer_size = 512;
static constexpr uint16_t _buffer_size = 512;
mString<_buffer_size> _buffer;
};

2
src/Sensors/Barometer.hpp
View File

@@ -75,6 +75,6 @@ private:
uint32_t _calibrationTimer = 0;
int _calibrationIterationsCounter = 0;
OnMeasuringFinishedCb _measuaringHeightFinishedCb = nullptr;
static constexpr int _calibrationIterationsCount = 1000;
static constexpr int _calibrationIterationsCount = 800;
static constexpr int _calibrationIterationDelay = 1; // [ms]
};

4
src/Sensors/RangingSensor.cpp
View File

@@ -5,7 +5,7 @@
#include "RangingSensor.hpp"
#include "esp_log.h"
RangingSensor::RangingSensor(TwoWire &i2c, volatile bool loop_on_fail) {
RangingSensor::RangingSensor(TwoWire &i2c) {
_sensor = VL53L0X();
_sensor.setBus(&i2c);
if (_sensor.init()) {
@@ -14,7 +14,7 @@ RangingSensor::RangingSensor(TwoWire &i2c, volatile bool loop_on_fail) {
ESP_LOGI(_tag, "Sensor initialized");
} else {
ESP_LOGE(_tag, "Failed to initialize RangingSensor!");
while (loop_on_fail) {}
assert(false);
}
_sensor.startContinuous(10);
}

2
src/Sensors/RangingSensor.hpp
View File

@@ -10,7 +10,7 @@
class RangingSensor {
public:
RangingSensor(TwoWire &i2c, volatile bool loop_on_fail = true);
RangingSensor(TwoWire &i2c);
void tick();

15
src/Sensors/Sensors.cpp
View File

@@ -4,7 +4,7 @@
#include "Sensors.hpp"
Sensors::Sensors(Barometer *barometer, RangingSensor *ranging_sensor, MPU *mpu, Kalman2DFilter *filter,
BatteryController *battery, HeightSensor heightSensor, volatile bool loop_on_fail) {
BatteryController *battery, HeightSensor heightSensor) {
assert(barometer != nullptr);
assert(mpu != nullptr);
assert(filter != nullptr);
@@ -23,7 +23,7 @@ Sensors::Sensors(Barometer *barometer, RangingSensor *ranging_sensor, MPU *mpu,
ESP_LOGI(_tag, "BMP280 initialized");
} else {
ESP_LOGI(_tag, "BMP280 initialization failed!");
while (loop_on_fail);
assert(false);
}
ESP_LOGI(_tag, "Initialize MPU6050...");
@@ -31,7 +31,7 @@ Sensors::Sensors(Barometer *barometer, RangingSensor *ranging_sensor, MPU *mpu,
ESP_LOGI(_tag, "MPU6050 initialized");
} else {
ESP_LOGI(_tag, "MPU6050 initialization failed!");
while (loop_on_fail);
assert(false);
}
_battery->initialize();
@@ -53,6 +53,12 @@ void Sensors::measureBaseAltitudeSync(void) {
}
void Sensors::measureBaseAltitudeAsync(void) {
if (_heightSensor == HeightSensor::RangeMeterOnly) {
if (_onMeasuaringAltitudeFinished) {
_onMeasuaringAltitudeFinished();
}
return;
}
_barometer->measureBaseAltitudeAsync();
}
@@ -129,5 +135,6 @@ void Sensors::startMpuCalibration() {
}
void Sensors::onMeasuaringAltitudeFinished(OnMeasuringFinishedCb callback) {
_barometer->onMeasuaringHeightFinished(callback);
_onMeasuaringAltitudeFinished = callback;
_barometer->onMeasuaringHeightFinished(std::move(callback));
}

3
src/Sensors/Sensors.hpp
View File

@@ -27,7 +27,7 @@ enum HeightSensor {
class Sensors {
public:
Sensors(Barometer *barometer, RangingSensor *rangingSensor, MPU *mpu, Kalman2DFilter *filter,
BatteryController *battery, HeightSensor heightSensor = Hybrid, bool loop_on_fail = true);
BatteryController *battery, HeightSensor heightSensor = Hybrid);
~Sensors();
@@ -58,6 +58,7 @@ private:
Kalman2DFilter *_2d_filter = nullptr;
RangingSensor *_ranging_sensor = nullptr;
HeightSensor _heightSensor;
OnMeasuringFinishedCb _onMeasuaringAltitudeFinished = nullptr;
/* cached filtered values */
float _flightHeightFromBarometer = 0.f;

2
src/main.cpp
View File

@@ -18,7 +18,7 @@ void setup() {
new MPU(new MPU6050(MPU6050_DEFAULT_ADDRESS, &i2c)),
new Kalman2DFilter(10.f, 1.f, 1.8f),
new BatteryController(BATTERY_DATA_PIN, BATTERY_DATA_SWITCH_PIN),
HeightSensor::Hybrid),
HeightSensor::RangeMeterOnly),
new SavedPidRegulator(3.5f, 0.0015f, 0.01f, "HeightControl"), // height
new SavedPidRegulator(2.f, 12.f, 0.f, "YawControl"), // yaw
new BrushedMotor(1, 2, 3, 4, 1),