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

Cleanups

Browse Source
This commit is contained in:
Oleg Kalachev 2023-05-24 10:56:59 +03:00
parent 87c75842f9
commit 82276ddb92
6 changed files with 11 additions and 42 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
flix/cli.ino
View File

@ -1,7 +1,6 @@
// Copyright (c) 2023 Oleg Kalachev <okalachev@gmail.com>
// Repository: https://github.com/okalachev/flix
#include <MPU9250.h>
#include "pid.h"
static String command;
@ -9,7 +8,6 @@ static String value;
static bool parsingCommand = true;
extern PID rollRatePID, pitchRatePID, yawRatePID, rollPID, pitchPID;
extern MPU9250 IMU;
const char* motd =
"\nWelcome to\n"
@ -68,7 +66,7 @@ static void doCommand()
} else if (command == "psq") {
Serial.printf("qx: %f qy: %f qz: %f qw: %f\n", attitude.x, attitude.y, attitude.z, attitude.w);
} else if (command == "imu") {
Serial.printf("gyro bias %f %f %f\n", IMU.getGyroBiasX_rads(), IMU.getGyroBiasY_rads(), IMU.getGyroBiasZ_rads());
printIMUCal();
} else if (command == "rc") {
Serial.printf("RAW throttle %d yaw %d pitch %d roll %d aux %d mode %d\n",
channels[RC_CHANNEL_THROTTLE], channels[RC_CHANNEL_YAW], channels[RC_CHANNEL_PITCH],

2
flix/flix.ino
View File

@ -21,7 +21,6 @@
#define MOTOR_FRONT_RIGHT 2
#define MOTOR_REAR_RIGHT 1
uint32_t startTime; // system startup time
uint32_t stepTime; // current step time
uint32_t steps; // total steps count
float stepsPerSecond; // steps per last second
@ -33,7 +32,6 @@ float motors[4]; // normalized motors thrust in range [-1..1]
Vector rates; // angular rates, rad/s
Vector acc; // accelerometer data, m/s/s
Quaternion attitude; // estimated attitude
bool calibrating; // flag we're calibrating
void setup()
{

23
flix/imu.ino
View File

@ -55,17 +55,12 @@ bool readIMU()
static void calibrateGyro()
{
calibrating = true;
Serial.println("Calibrating gyro, stand still");
delay(500);
int status = IMU.calibrateGyro();
Serial.printf("Calibration status: %d\n", status);
Serial.print("Gyro bias: ");
Serial.print(IMU.getGyroBiasX_rads(), 10); Serial.print(" ");
Serial.print(IMU.getGyroBiasY_rads(), 10); Serial.print(" ");
Serial.println(IMU.getGyroBiasZ_rads(), 10);
IMU.setSrd(0);
calibrating = false;
printIMUCal();
}
static void calibrateAccel()
@ -82,14 +77,7 @@ static void calibrateAccel()
IMU.calibrateAccel();
Serial.println("Cal accel: upside down"); delay(300);
IMU.calibrateAccel();
Serial.print("Accel bias: ");
Serial.print(IMU.getAccelBiasX_mss(), 10); Serial.print(" ");
Serial.print(IMU.getAccelBiasY_mss(), 10); Serial.print(" ");
Serial.println(IMU.getAccelBiasZ_mss(), 10);
Serial.print("Accel scale: ");
Serial.print(IMU.getAccelScaleFactorX(), 10); Serial.print(" ");
Serial.print(IMU.getAccelScaleFactorY(), 10); Serial.print(" ");
Serial.println(IMU.getAccelScaleFactorZ(), 10);
printIMUCal();
}
static void loadAccelCal()
@ -106,6 +94,13 @@ static void loadGyroCal()
IMU.setGyroBiasZ_rads(0.0163032326);
}
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());
}
// Accel bias: 0.0463809967 0.0463809967 0.1486964226
// Accel scale: 0.9986976385 0.9993721247 1.0561490059

2
flix/mavlink.ino
View File

@ -21,7 +21,7 @@ void sendMavlink()
mavlink_msg_heartbeat_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg, MAV_TYPE_QUADROTOR,
MAV_AUTOPILOT_GENERIC, MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_SAFETY_ARMED,
0, calibrating ? MAV_STATE_CALIBRATING : MAV_STATE_STANDBY);
0, MAV_STATE_STANDBY);
sendMessage(&msg);
// params test

17
gazebo/MPU9250.h
View File

@ -1,17 +0,0 @@
// Copyright (c) 2023 Oleg Kalachev <okalachev@gmail.com>
// Repository: https://github.com/okalachev/flix
// Distributed under the MIT License (https://opensource.org/licenses/MIT)
// Mocks of some MPU9250 library functions
#pragma once
class MPU9250 {
public:
float getAccelBiasX_mss() { return 0; }
float getAccelBiasY_mss() { return 0; }
float getAccelBiasZ_mss() { return 0; }
float getGyroBiasX_rads() { return 0; }
float getGyroBiasY_rads() { return 0; }
float getGyroBiasZ_rads() { return 0; }
};

5
gazebo/flix.cpp
View File

@ -140,11 +140,6 @@ public:
const double maxThrust = 0.03 * ONE_G; // 30 g, https://www.youtube.com/watch?v=VtKI4Pjx8Sk
// 65 mm prop ~40 g
// std::cout << "fr: " << motors[MOTOR_FRONT_RIGHT]
// << " fl: " << motors[MOTOR_FRONT_LEFT]
// << " rr: " << motors[MOTOR_REAR_RIGHT]
// << " rl: " << motors[MOTOR_REAR_LEFT] << std::endl;
const float scale0 = 1.0, scale1 = 1.1, scale2 = 0.9, scale3 = 1.05;
const float minThrustRel = 0;