aleks/flix
1
0
Fork 0
mirror of https://github.com/okalachev/flix.git synced 2025-07-27 17:49:33 +00:00
Code Issues Packages Projects Releases Wiki Activity

Change C++ code style: put curly brace on the same line

Browse Source
This commit is contained in:
Oleg Kalachev 2023-12-29 18:56:25 +03:00
parent 645b148564
commit d2296fea76
19 changed files with 65 additions and 130 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
flix/cli.ino
View File

@ -57,8 +57,7 @@ const struct Param {
{"t", &t, nullptr},
};
void doCommand(String& command, String& value)
{
void doCommand(String& command, String& value) {
if (command == "help" || command == "motd") {
Serial.println(motd);
} else if (command == "show") {
@ -119,8 +118,7 @@ void doCommand(String& command, String& value)
}
}
void showTable()
{
void showTable() {
for (uint8_t i = 0; i < sizeof(params) / sizeof(params[0]); i++) {
Serial.print(params[i].name);
Serial.print(" ");
@ -128,8 +126,7 @@ void showTable()
}
}
void cliTestMotor(uint8_t n)
{
void cliTestMotor(uint8_t n) {
Serial.printf("Testing motor %d\n", n);
motors[n] = 1;
sendMotors();
@ -139,8 +136,7 @@ void cliTestMotor(uint8_t n)
Serial.println("Done");
}
void parseInput()
{
void parseInput() {
static bool showMotd = true;
static String command;
static String value;

21
flix/control.ino
View File

@ -53,8 +53,7 @@ Vector ratesTarget;
Vector torqueTarget;
float thrustTarget;
void control()
{
void control() {
interpretRC();
if (mode == STAB) {
controlAttitude();
@ -68,8 +67,7 @@ void control()
}
}
void interpretRC()
{
void interpretRC() {
if (controls[RC_CHANNEL_MODE] < 0.25) {
mode = MANUAL;
} else if (controls[RC_CHANNEL_MODE] < 0.75) {
@ -108,8 +106,7 @@ void interpretRC()
}
}
void controlAttitude()
{
void controlAttitude() {
if (!armed) {
rollPID.reset();
pitchPID.reset();
@ -131,8 +128,7 @@ void controlAttitude()
}
}
void controlRate()
{
void controlRate() {
if (!armed) {
rollRatePID.reset();
pitchRatePID.reset();
@ -147,8 +143,7 @@ void controlRate()
torqueTarget.z = yawRatePID.update(ratesTarget.z - ratesFiltered.z, dt);
}
void controlTorque()
{
void controlTorque() {
if (!armed) {
memset(motors, 0, sizeof(motors));
return;
@ -165,13 +160,11 @@ void controlTorque()
motors[3] = constrain(motors[3], 0, 1);
}
bool motorsActive()
{
bool motorsActive() {
return motors[0] > 0 || motors[1] > 0 || motors[2] > 0 || motors[3] > 0;
}
const char* getModeName()
{
const char* getModeName() {
switch (mode) {
case MANUAL: return "MANUAL";
case ACRO: return "ACRO";

12
flix/estimate.ino
View File

@ -9,22 +9,19 @@
#define ONE_G 9.807f
#define WEIGHT_ACC 0.5f
void estimate()
{
void estimate() {
applyGyro();
applyAcc();
signalizeHorizontality();
}
void applyGyro()
{
void applyGyro() {
// applying gyro
attitude *= Quaternion::fromAngularRates(rates * dt);
attitude.normalize();
}
void applyAcc()
{
void applyAcc() {
// test should we apply accelerometer gravity correction
float accNorm = acc.norm();
bool landed = !motorsActive() && abs(accNorm - ONE_G) < ONE_G * 0.1f;
@ -40,8 +37,7 @@ void applyAcc()
attitude.normalize();
}
void signalizeHorizontality()
{
void signalizeHorizontality() {
float angle = Vector::angleBetweenVectors(attitude.rotate(Vector(0, 0, -1)), Vector(0, 0, -1));
setLED(angle < 15 * DEG_TO_RAD);
}

6
flix/flix.ino
View File

@ -33,8 +33,7 @@ Vector acc; // accelerometer data, m/s/s
Quaternion attitude; // estimated attitude
float motors[4]; // normalized motors thrust in range [-1..1]
void setup()
{
void setup() {
Serial.begin(SERIAL_BAUDRATE);
Serial.println("Initializing flix");
setupLED();
@ -50,8 +49,7 @@ void setup()
Serial.println("Initializing complete");
}
void loop()
{
void loop() {
if (!readIMU()) return;
step();

21
flix/imu.ino
View File

@ -11,8 +11,7 @@
MPU9250 IMU(SPI, IMU_CS_PIN);
void setupIMU()
{
void setupIMU() {
Serial.println("Setup IMU");
auto status = IMU.begin();
@ -35,8 +34,7 @@ void setupIMU()
// NOTE: very important, without the above the rate would be terrible 50 Hz
}
bool readIMU()
{
bool readIMU() {
if (IMU.readSensor() < 0) {
Serial.println("IMU read error");
return false;
@ -54,8 +52,7 @@ bool readIMU()
return rates != lastRates;
}
void calibrateGyro()
{
void calibrateGyro() {
Serial.println("Calibrating gyro, stand still");
delay(500);
int status = IMU.calibrateGyro();
@ -64,8 +61,7 @@ void calibrateGyro()
printIMUCal();
}
void calibrateAccel()
{
void calibrateAccel() {
Serial.println("Cal accel: place level"); delay(3000);
IMU.calibrateAccel();
Serial.println("Cal accel: place nose up"); delay(3000);
@ -81,24 +77,21 @@ void calibrateAccel()
printIMUCal();
}
void loadAccelCal()
{
void loadAccelCal() {
// NOTE: this should be changed to the actual values
IMU.setAccelCalX(-0.0048542023, 1.0008112192);
IMU.setAccelCalY(0.0521845818, 0.9985780716);
IMU.setAccelCalZ(0.5754694939, 1.0045746565);
}
void loadGyroCal()
{
void loadGyroCal() {
// NOTE: this should be changed to the actual values
IMU.setGyroBiasX_rads(-0.0185128022);
IMU.setGyroBiasY_rads(-0.0262369743);
IMU.setGyroBiasZ_rads(0.0163032326);
}
void printIMUCal()
{
void printIMUCal() {
Serial.printf("gyro bias: %f %f %f\n", IMU.getGyroBiasX_rads(), IMU.getGyroBiasY_rads(), IMU.getGyroBiasZ_rads());
Serial.printf("accel bias: %f %f %f\n", IMU.getAccelBiasX_mss(), IMU.getAccelBiasY_mss(), IMU.getAccelBiasZ_mss());
Serial.printf("accel scale: %f %f %f\n", IMU.getAccelScaleFactorX(), IMU.getAccelScaleFactorY(), IMU.getAccelScaleFactorZ());

9
flix/led.ino
View File

@ -5,17 +5,14 @@
#define BLINK_PERIOD 500000
void setupLED()
{
void setupLED() {
pinMode(LED_BUILTIN, OUTPUT);
}
void setLED(bool on)
{
void setLED(bool on) {
digitalWrite(LED_BUILTIN, on ? HIGH : LOW);
}
void blinkLED()
{
void blinkLED() {
setLED(micros() / BLINK_PERIOD % 2);
}

6
flix/log.ino
View File

@ -12,8 +12,7 @@
float logBuffer[LOG_SIZE][LOG_COLUMNS]; // * 4 (float)
int logPointer = 0;
void logData()
{
void logData() {
if (!armed) return;
static float logTime = 0;
@ -41,8 +40,7 @@ void logData()
}
}
void dumpLog()
{
void dumpLog() {
Serial.printf("t,rates.x,rates.y,rates.z,ratesTarget.x,ratesTarget.y,ratesTarget.z,"
"attitude.x,attitude.y,attitude.z,attitudeTarget.x,attitudeTarget.y,attitudeTarget.z,thrustTarget\n");
for (int i = 0; i < LOG_SIZE; i++) {

3
flix/lpf.h
View File

@ -6,8 +6,7 @@
#pragma once
template <typename T> // Using template to make the filter usable for scalar and vector values
class LowPassFilter
{
class LowPassFilter {
public:
float alpha; // smoothing constant, 1 means filter disabled
T output;

6
flix/mavlink.ino
View File

@ -11,8 +11,7 @@
#define PERIOD_SLOW 1.0
#define PERIOD_FAST 0.1
void sendMavlink()
{
void sendMavlink() {
static float lastSlow = 0;
static float lastFast = 0;
@ -55,8 +54,7 @@ void sendMavlink()
}
}
inline void sendMessage(const void *msg)
{
inline void sendMessage(const void *msg) {
uint8_t buf[MAVLINK_MAX_PACKET_LEN];
uint16_t len = mavlink_msg_to_send_buffer(buf, (mavlink_message_t *)msg);
sendWiFi(buf, len);

9
flix/motors.ino
View File

@ -39,8 +39,7 @@ void setupMotors() {
Serial.println("Motors initialized");
}
uint16_t getPWM(float val, int n)
{
uint16_t getPWM(float val, int n) {
if (val == 0) {
return PWM_NEUTRAL;
} else if (val > 0) {
@ -54,16 +53,14 @@ uint8_t pwmToDutyCycle(uint16_t pwm) {
return map(pwm, 0, 1000000 / PWM_FREQUENCY, 0, (1 << PWM_RESOLUTION) - 1);
}
void sendMotors()
{
void sendMotors() {
ledcWrite(0, pwmToDutyCycle(getPWM(motors[0], 0)));
ledcWrite(1, pwmToDutyCycle(getPWM(motors[1], 1)));
ledcWrite(2, pwmToDutyCycle(getPWM(motors[2], 2)));
ledcWrite(3, pwmToDutyCycle(getPWM(motors[3], 3)));
}
void fullMotorTest(int n, bool reverse)
{
void fullMotorTest(int n, bool reverse) {
printf("Full test for motor %d\n", n);
for (int pwm = PWM_NEUTRAL; pwm <= 2300 && pwm >= 700; pwm += reverse ? -100 : 100) {
printf("Motor %d: %d\n", n, pwm);

3
flix/pid.h
View File

@ -7,8 +7,7 @@
#include "lpf.h"
class PID
{
class PID {
public:
float p = 0;
float i = 0;

6
flix/rc.ino
View File

@ -11,14 +11,12 @@ const uint16_t channelMax[] = {1651, 1540, 1713, 1630, 1472, 1472};
SBUS RC(Serial2);
void setupRC()
{
void setupRC() {
Serial.println("Setup RC");
RC.begin();
}
void readRC()
{
void readRC() {
bool failSafe, lostFrame;
if (RC.read(channels, &failSafe, &lostFrame)) {
if (failSafe) { return; } // TODO:

6
flix/time.ino
View File

@ -3,8 +3,7 @@
// Time related functions
void step()
{
void step() {
float now = micros() / 1000000.0;
dt = now - t;
t = now;
@ -16,8 +15,7 @@ void step()
computeLoopFreq();
}
void computeLoopFreq()
{
void computeLoopFreq() {
static float windowStart = 0;
static uint32_t freq = 0;
freq++;

15
flix/util.ino
View File

@ -5,29 +5,24 @@
#include "math.h"
float mapf(long x, long in_min, long in_max, float out_min, float out_max)
{
float mapf(long x, long in_min, long in_max, float out_min, float out_max) {
return (float)(x - in_min) * (out_max - out_min) / (float)(in_max - in_min) + out_min;
}
float mapff(float x, float in_min, float in_max, float out_min, float out_max)
{
float mapff(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;
}
int8_t sign(float x)
{
int8_t sign(float x) {
return (x > 0) - (x < 0);
}
float randomFloat(float min, float max)
{
float randomFloat(float min, float max) {
return min + (max - min) * (float)rand() / RAND_MAX;
}
// wrap angle to [-PI, PI)
float wrapAngle(float angle)
{
float wrapAngle(float angle) {
angle = fmodf(angle, 2 * PI);
if (angle > PI) {
angle -= 2 * PI;

3
flix/vector.h
View File

@ -5,8 +5,7 @@
#pragma once
class Vector : public Printable
{
class Vector : public Printable {
public:
float x, y, z;

6
flix/wifi.ino
View File

@ -18,15 +18,13 @@
WiFiUDP udp;
void setupWiFi()
{
void setupWiFi() {
Serial.println("Setup Wi-Fi");
WiFi.softAP(WIFI_SSID, WIFI_PASSWORD);
IPAddress myIP = WiFi.softAPIP();
}
inline void sendWiFi(const uint8_t *buf, size_t len)
{
inline void sendWiFi(const uint8_t *buf, size_t len) {
udp.beginPacket(WIFI_UDP_IP, WIFI_UDP_PORT);
udp.write(buf, len);
udp.endPacket();

30
gazebo/Arduino.h
View File

@ -44,8 +44,7 @@ public:
class Print {
public:
size_t printf(const char *format, ...)
{
size_t printf(const char *format, ...) {
va_list args;
va_start(args, format);
size_t result = vprintf(format, args);
@ -53,48 +52,39 @@ public:
return result;
}
size_t print(float n, int digits = 2)
{
size_t print(float n, int digits = 2) {
return printf("%.*f", digits, n);
}
size_t println(float n, int digits = 2)
{
size_t println(float n, int digits = 2) {
return printf("%.*f\n", digits, n);
}
size_t print(const char* s)
{
size_t print(const char* s) {
return printf("%s", s);
}
size_t println()
{
size_t println() {
return print("\n");
}
size_t println(const char* s)
{
size_t println(const char* s) {
return printf("%s\n", s);
}
size_t println(const Printable& p)
{
size_t println(const Printable& p) {
return p.printTo(*this) + print("\n");
}
size_t print(const String& s)
{
size_t print(const String& s) {
return printf("%s", s.c_str());
}
size_t println(const std::string& s)
{
size_t println(const std::string& s) {
return printf("%s\n", s.c_str());
}
size_t println(const String& s)
{
size_t println(const String& s) {
return printf("%s\n", s.c_str());
}
};

6
gazebo/joystick.h
View File

@ -24,8 +24,7 @@ bool joystickInitialized = false, warnShown = false;
void normalizeRC();
void joystickInit()
{
void joystickInit() {
SDL_Init(SDL_INIT_JOYSTICK);
joystick = SDL_JoystickOpen(0);
if (joystick != NULL) {
@ -37,8 +36,7 @@ void joystickInit()
}
}
void joystickGet()
{
void joystickGet() {
if (!joystickInitialized) {
joystickInit();
return;

15
gazebo/simulator.cpp
View File

@ -33,8 +33,7 @@ using ignition::math::Vector3d;
using namespace gazebo;
using namespace std;
class ModelFlix : public ModelPlugin
{
class ModelFlix : public ModelPlugin {
private:
physics::ModelPtr model;
physics::LinkPtr body;
@ -45,8 +44,7 @@ private:
LowPassFilter<Vector> accFilter = LowPassFilter<Vector>(0.1);
public:
void Load(physics::ModelPtr _parent, sdf::ElementPtr /*_sdf*/)
{
void Load(physics::ModelPtr _parent, sdf::ElementPtr /*_sdf*/) {
this->model = _parent;
this->body = this->model->GetLink("body");
this->imu = dynamic_pointer_cast<sensors::ImuSensor>(sensors::get_sensor(model->GetScopedName(true) + "::body::imu")); // default::flix::body::imu
@ -58,14 +56,12 @@ public:
}
public:
void OnReset()
{
void OnReset() {
attitude = Quaternion(); // reset estimated attitude
gzmsg << "Flix plugin reset" << endl;
}
void OnUpdate()
{
void OnUpdate() {
__micros = model->GetWorld()->SimTime().Double() * 1000000;
step();
@ -91,8 +87,7 @@ public:
logData();
}
void applyMotorsThrust()
{
void applyMotorsThrust() {
// thrusts
const double dist = 0.035355; // motors shift from the center, m
const double maxThrust = 0.03 * ONE_G; // ~30 g, https://youtu.be/VtKI4Pjx8Sk?&t=78