mirror of
https://github.com/okalachev/flix.git
synced 2026-01-11 05:26:53 +00:00
8418723ccc
Add interpretControls function to convert pilot commands and mode into control targets and make control functions independent from the mode. Add ratesExtra target for rates feed-forward; remove yawMode. Rename controlRate to controlRates to reflect rates variable name. Remove USER mode.
55 lines
1.2 KiB
C++
55 lines
1.2 KiB
C++
// Copyright (c) 2023 Oleg Kalachev <okalachev@gmail.com>
|
|
// Repository: https://github.com/okalachev/flix
|
|
|
|
// Main firmware file
|
|
|
|
#include "vector.h"
|
|
#include "quaternion.h"
|
|
#include "util.h"
|
|
|
|
#define SERIAL_BAUDRATE 115200
|
|
#define WIFI_ENABLED 1
|
|
|
|
double t = NAN; // current step time, s
|
|
float dt; // time delta from previous step, s
|
|
float controlRoll, controlPitch, controlYaw, controlThrottle; // pilot's inputs, range [-1, 1]
|
|
float controlArmed = NAN, controlMode = NAN;
|
|
Vector gyro; // gyroscope data
|
|
Vector acc; // accelerometer data, m/s/s
|
|
Vector rates; // filtered angular rates, rad/s
|
|
Quaternion attitude; // estimated attitude
|
|
bool landed; // are we landed and stationary
|
|
float motors[4]; // normalized motors thrust in range [0..1]
|
|
|
|
void setup() {
|
|
Serial.begin(SERIAL_BAUDRATE);
|
|
print("Initializing flix\n");
|
|
disableBrownOut();
|
|
setupParameters();
|
|
setupLED();
|
|
setupMotors();
|
|
setLED(true);
|
|
#if WIFI_ENABLED
|
|
setupWiFi();
|
|
#endif
|
|
setupIMU();
|
|
setupRC();
|
|
setLED(false);
|
|
print("Initializing complete\n");
|
|
}
|
|
|
|
void loop() {
|
|
readIMU();
|
|
step();
|
|
readRC();
|
|
estimate();
|
|
control();
|
|
sendMotors();
|
|
handleInput();
|
|
#if WIFI_ENABLED
|
|
processMavlink();
|
|
#endif
|
|
logData();
|
|
syncParameters();
|
|
}
|