mirror of
https://github.com/okalachev/flix.git
synced 2025-07-27 17:49:33 +00:00
Remove non-effective static
s
This commit is contained in:
parent
9a93367629
commit
39875cafb9
@ -30,7 +30,7 @@ const char* motd =
|
|||||||
"fullmot <n> - test motor on all signals\n"
|
"fullmot <n> - test motor on all signals\n"
|
||||||
"wifi - start wi-fi access point\n";
|
"wifi - start wi-fi access point\n";
|
||||||
|
|
||||||
static const struct Param {
|
const struct Param {
|
||||||
const char* name;
|
const char* name;
|
||||||
float* value;
|
float* value;
|
||||||
float* value2;
|
float* value2;
|
||||||
@ -51,7 +51,7 @@ static const struct Param {
|
|||||||
// {"m", &mode, nullptr},
|
// {"m", &mode, nullptr},
|
||||||
};
|
};
|
||||||
|
|
||||||
static void doCommand(String& command, String& value)
|
void doCommand(String& command, String& value)
|
||||||
{
|
{
|
||||||
if (command == "help" || command == "motd") {
|
if (command == "help" || command == "motd") {
|
||||||
Serial.println(motd);
|
Serial.println(motd);
|
||||||
@ -110,7 +110,7 @@ static void doCommand(String& command, String& value)
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
static void showTable()
|
void showTable()
|
||||||
{
|
{
|
||||||
for (uint8_t i = 0; i < sizeof(params) / sizeof(params[0]); i++) {
|
for (uint8_t i = 0; i < sizeof(params) / sizeof(params[0]); i++) {
|
||||||
Serial.print(params[i].name);
|
Serial.print(params[i].name);
|
||||||
@ -119,7 +119,7 @@ static void showTable()
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
static void cliTestMotor(uint8_t n)
|
void cliTestMotor(uint8_t n)
|
||||||
{
|
{
|
||||||
Serial.printf("Testing motor %d\n", n);
|
Serial.printf("Testing motor %d\n", n);
|
||||||
motors[n] = 1;
|
motors[n] = 1;
|
||||||
|
@ -98,7 +98,7 @@ void interpretRC()
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
static void controlAttitude()
|
void controlAttitude()
|
||||||
{
|
{
|
||||||
if (!armed) {
|
if (!armed) {
|
||||||
rollPID.reset();
|
rollPID.reset();
|
||||||
@ -149,7 +149,7 @@ static void controlAttitude()
|
|||||||
// std::cout << "rsp: " << ratesTarget.x << " " << ratesTarget.y << std::endl;
|
// std::cout << "rsp: " << ratesTarget.x << " " << ratesTarget.y << std::endl;
|
||||||
}
|
}
|
||||||
|
|
||||||
static void controlAttitudeAlter()
|
void controlAttitudeAlter()
|
||||||
{
|
{
|
||||||
if (!armed) {
|
if (!armed) {
|
||||||
rollPID.reset();
|
rollPID.reset();
|
||||||
@ -169,7 +169,7 @@ static void controlAttitudeAlter()
|
|||||||
}
|
}
|
||||||
|
|
||||||
// passthrough mode
|
// passthrough mode
|
||||||
static void controlManual()
|
void controlManual()
|
||||||
{
|
{
|
||||||
if (controls[RC_CHANNEL_THROTTLE] < 0.1) {
|
if (controls[RC_CHANNEL_THROTTLE] < 0.1) {
|
||||||
memset(motors, 0, sizeof(motors));
|
memset(motors, 0, sizeof(motors));
|
||||||
@ -195,7 +195,7 @@ static void controlManual()
|
|||||||
motors[3] = constrain(motors[3], 0, 1);
|
motors[3] = constrain(motors[3], 0, 1);
|
||||||
}
|
}
|
||||||
|
|
||||||
static void controlRate()
|
void controlRate()
|
||||||
{
|
{
|
||||||
if (!armed) { // TODO: too rough
|
if (!armed) { // TODO: too rough
|
||||||
memset(motors, 0, sizeof(motors));
|
memset(motors, 0, sizeof(motors));
|
||||||
@ -257,7 +257,7 @@ void desaturate(float& a, float& b, float& c, float& d)
|
|||||||
|
|
||||||
static bool motorsSaturation = false;
|
static bool motorsSaturation = false;
|
||||||
|
|
||||||
static inline void indicateSaturation() {
|
inline void indicateSaturation() {
|
||||||
bool sat = motors[0] > 1 || motors[1] > 1 || motors[2] > 1 || motors[3] > 1 ||
|
bool sat = motors[0] > 1 || motors[1] > 1 || motors[2] > 1 || motors[3] > 1 ||
|
||||||
motors[0] < 0 || motors[1] < 0 || motors[2] < 0 || motors[3] < 0;
|
motors[0] < 0 || motors[1] < 0 || motors[2] < 0 || motors[3] < 0;
|
||||||
if (motorsSaturation != sat) {
|
if (motorsSaturation != sat) {
|
||||||
|
@ -53,7 +53,7 @@ bool readIMU()
|
|||||||
return rates != lastRates;
|
return rates != lastRates;
|
||||||
}
|
}
|
||||||
|
|
||||||
static void calibrateGyro()
|
void calibrateGyro()
|
||||||
{
|
{
|
||||||
Serial.println("Calibrating gyro, stand still");
|
Serial.println("Calibrating gyro, stand still");
|
||||||
delay(500);
|
delay(500);
|
||||||
@ -63,7 +63,7 @@ static void calibrateGyro()
|
|||||||
printIMUCal();
|
printIMUCal();
|
||||||
}
|
}
|
||||||
|
|
||||||
static void calibrateAccel()
|
void calibrateAccel()
|
||||||
{
|
{
|
||||||
Serial.println("Cal accel: place level"); delay(3000);
|
Serial.println("Cal accel: place level"); delay(3000);
|
||||||
IMU.calibrateAccel();
|
IMU.calibrateAccel();
|
||||||
@ -80,14 +80,14 @@ static void calibrateAccel()
|
|||||||
printIMUCal();
|
printIMUCal();
|
||||||
}
|
}
|
||||||
|
|
||||||
static void loadAccelCal()
|
void loadAccelCal()
|
||||||
{
|
{
|
||||||
IMU.setAccelCalX(-0.0048542023, 1.0008112192);
|
IMU.setAccelCalX(-0.0048542023, 1.0008112192);
|
||||||
IMU.setAccelCalY(0.0521845818, 0.9985780716);
|
IMU.setAccelCalY(0.0521845818, 0.9985780716);
|
||||||
IMU.setAccelCalZ(0.5754694939, 1.0045746565);
|
IMU.setAccelCalZ(0.5754694939, 1.0045746565);
|
||||||
}
|
}
|
||||||
|
|
||||||
static void loadGyroCal()
|
void loadGyroCal()
|
||||||
{
|
{
|
||||||
IMU.setGyroBiasX_rads(-0.0185128022);
|
IMU.setGyroBiasX_rads(-0.0185128022);
|
||||||
IMU.setGyroBiasY_rads(-0.0262369743);
|
IMU.setGyroBiasY_rads(-0.0262369743);
|
||||||
|
@ -9,8 +9,8 @@ const float LOG_PERIOD = 1 / LOG_RATE;
|
|||||||
const int LOG_SIZE = LOG_DURATION * LOG_RATE;
|
const int LOG_SIZE = LOG_DURATION * LOG_RATE;
|
||||||
const int LOG_COLUMNS = 14;
|
const int LOG_COLUMNS = 14;
|
||||||
|
|
||||||
static float logBuffer[LOG_SIZE][LOG_COLUMNS]; // * 4 (float)
|
float logBuffer[LOG_SIZE][LOG_COLUMNS]; // * 4 (float)
|
||||||
static int logPointer = 0;
|
int logPointer = 0;
|
||||||
|
|
||||||
void logData()
|
void logData()
|
||||||
{
|
{
|
||||||
|
@ -9,8 +9,8 @@
|
|||||||
#define PERIOD_SLOW 1000000 // us
|
#define PERIOD_SLOW 1000000 // us
|
||||||
#define PERIOD_FAST 100000 // us
|
#define PERIOD_FAST 100000 // us
|
||||||
|
|
||||||
static uint32_t lastSlow;
|
uint32_t lastSlow;
|
||||||
static uint32_t lastFast;
|
uint32_t lastFast;
|
||||||
|
|
||||||
void sendMavlink()
|
void sendMavlink()
|
||||||
{
|
{
|
||||||
@ -60,7 +60,7 @@ void sendMavlink()
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
static inline void sendMessage(const void *msg)
|
inline void sendMessage(const void *msg)
|
||||||
{
|
{
|
||||||
uint8_t buf[MAVLINK_MAX_PACKET_LEN];
|
uint8_t buf[MAVLINK_MAX_PACKET_LEN];
|
||||||
uint16_t len = mavlink_msg_to_send_buffer(buf, (mavlink_message_t *)msg);
|
uint16_t len = mavlink_msg_to_send_buffer(buf, (mavlink_message_t *)msg);
|
||||||
|
@ -3,10 +3,10 @@
|
|||||||
|
|
||||||
#include <SBUS.h>
|
#include <SBUS.h>
|
||||||
|
|
||||||
static const uint16_t channelNeutral[] = {995, 883, 200, 972, 512, 512};
|
const uint16_t channelNeutral[] = {995, 883, 200, 972, 512, 512};
|
||||||
static const uint16_t channelMax[] = {1651, 1540, 1713, 1630, 1472, 1472};
|
const uint16_t channelMax[] = {1651, 1540, 1713, 1630, 1472, 1472};
|
||||||
|
|
||||||
static SBUS RC(Serial2);
|
SBUS RC(Serial2);
|
||||||
|
|
||||||
void setupRC()
|
void setupRC()
|
||||||
{
|
{
|
||||||
|
@ -1,7 +1,7 @@
|
|||||||
// Copyright (c) 2023 Oleg Kalachev <okalachev@gmail.com>
|
// Copyright (c) 2023 Oleg Kalachev <okalachev@gmail.com>
|
||||||
// Repository: https://github.com/okalachev/flix
|
// Repository: https://github.com/okalachev/flix
|
||||||
|
|
||||||
static void _pwm(int n, uint16_t pwm)
|
void _pwm(int n, uint16_t pwm)
|
||||||
{
|
{
|
||||||
printf("Motor %d: %d\n", n, pwm);
|
printf("Motor %d: %d\n", n, pwm);
|
||||||
ledcWrite(n, pwmToDutyCycle(pwm));
|
ledcWrite(n, pwmToDutyCycle(pwm));
|
||||||
|
@ -33,13 +33,13 @@ void applyAcc();
|
|||||||
void signalizeHorizontality();
|
void signalizeHorizontality();
|
||||||
void control();
|
void control();
|
||||||
void interpretRC();
|
void interpretRC();
|
||||||
static void controlAttitude();
|
void controlAttitude();
|
||||||
static void controlManual();
|
void controlManual();
|
||||||
static void controlRate();
|
void controlRate();
|
||||||
void desaturate(float& a, float& b, float& c, float& d);
|
void desaturate(float& a, float& b, float& c, float& d);
|
||||||
static void indicateSaturation();
|
void indicateSaturation();
|
||||||
static void showTable();
|
void showTable();
|
||||||
static void cliTestMotor(uint8_t n);
|
void cliTestMotor(uint8_t n);
|
||||||
|
|
||||||
// mocks
|
// mocks
|
||||||
void setLED(bool on) {};
|
void setLED(bool on) {};
|
||||||
|
Loading…
x
Reference in New Issue
Block a user