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

Major rework of rc subsystem

Browse Source 
Implement channels mapping calibration.
Store mapping in parameters.
Get rid of `controls` array and store control inputs in `controlRoll`, `controlPitch`, ... variables.
Move `channels` variable to rc.ino, channels are not involved when controled using mavlink.
'Neutral' values are renamed to 'zero' - more precise naming.
`controlsTime` is renamed to `controlTime`.
Use unsigned values for channels.
Make channel values in simulation more alike to real world: unsigned values in range [1000, 2000].
Send RC_CHANNELS_RAW instead of RC_CHANNELS_SCALED via mavlink
Don't send channels data via mavlink if rc is not used
This commit is contained in:
Oleg Kalachev 2025-07-14 11:11:32 +03:00
parent 449dd44741
commit 52819e403b
12 changed files with 137 additions and 103 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
docs/book/firmware.md
View File

@ -10,7 +10,7 @@
* `acc` *(Vector)* — данные с акселерометра, *м/с<sup>2</sup>*.
* `rates` *(Vector)* — отфильтрованные угловые скорости, *рад/с*.
* `attitude` *(Quaternion)* — оценка ориентации (положения) дрона.
* `controls` *(float[])* — команды управления от пилота, в диапазоне [-1, 1].
* `controlRoll`, `controlPitch`, ... *(float[])* — команды управления от пилота, в диапазоне [-1, 1].
* `motors` *(float[])* — выходные сигналы на моторы, в диапазоне [0, 1].
## Исходные файлы

2
docs/firmware.md
View File

@ -14,7 +14,7 @@ The main loop is running at 1000 Hz. All the dataflow is happening through globa
* `acc` *(Vector)* — acceleration data from the accelerometer, *m/s<sup>2</sup>*.
* `rates` *(Vector)* — filtered angular rates, *rad/s*.
* `attitude` *(Quaternion)* — estimated attitude (orientation) of drone.
* `controls` *(float[])* — pilot's control inputs, range [-1, 1].
* `controlRoll`, `controlPitch`, ... *(float[])* — pilot's control inputs, range [-1, 1].
* `motors` *(float[])* — motor outputs, range [0, 1].
## Source files

17
flix/cli.ino
View File

@ -10,7 +10,8 @@
extern const int MOTOR_REAR_LEFT, MOTOR_REAR_RIGHT, MOTOR_FRONT_RIGHT, MOTOR_FRONT_LEFT;
extern float loopRate, dt;
extern double t;
extern int rollChannel, pitchChannel, throttleChannel, yawChannel, armedChannel, modeChannel;
extern uint16_t channels[16];
extern float controlRoll, controlPitch, controlThrottle, controlYaw, controlArmed, controlMode;
const char* motd =
"\nWelcome to\n"
@ -109,13 +110,13 @@ void doCommand(String str, bool echo = false) {
print("rate: %.0f\n", loopRate);
print("landed: %d\n", landed);
} else if (command == "rc") {
print("Raw: throttle %d yaw %d pitch %d roll %d armed %d mode %d\n",
channels[throttleChannel], channels[yawChannel], channels[pitchChannel],
channels[rollChannel], channels[armedChannel], channels[modeChannel]);
print("Control: throttle %g yaw %g pitch %g roll %g armed %g mode %g\n",
controls[throttleChannel], controls[yawChannel], controls[pitchChannel],
controls[rollChannel], controls[armedChannel], controls[modeChannel]);
print("Mode: %s\n", getModeName());
print("channels: ");
for (int i = 0; i < 16; i++) {
print("%u ", channels[i]);
}
print("\nroll: %g pitch: %g yaw: %g throttle: %g armed: %g mode: %g\n",
controlRoll, controlPitch, controlYaw, controlThrottle, controlArmed, controlMode);
print("mode: %s\n", getModeName());
} else if (command == "mot") {
print("Motors: front-right %g front-left %g rear-right %g rear-left %g\n",
motors[MOTOR_FRONT_RIGHT], motors[MOTOR_FRONT_LEFT], motors[MOTOR_REAR_RIGHT], motors[MOTOR_REAR_LEFT]);

29
flix/control.ino
View File

@ -21,7 +21,7 @@
#define YAWRATE_I 0.0
#define YAWRATE_D 0.0
#define YAWRATE_I_LIM 0.3
#define ROLL_P 4.5
#define ROLL_P 6
#define ROLL_I 0
#define ROLL_D 0
#define PITCH_P ROLL_P
@ -54,7 +54,7 @@ Vector torqueTarget;
float thrustTarget;
extern const int MOTOR_REAR_LEFT, MOTOR_REAR_RIGHT, MOTOR_FRONT_RIGHT, MOTOR_FRONT_LEFT;
extern int rollChannel, pitchChannel, throttleChannel, yawChannel, armedChannel, modeChannel;
extern float controlRoll, controlPitch, controlThrottle, controlYaw, controlArmed, controlMode;
void control() {
interpretRC();
@ -72,39 +72,38 @@ void control() {
}
void interpretRC() {
armed = controls[throttleChannel] >= 0.05 &&
(controls[armedChannel] >= 0.5 || isnan(controls[armedChannel])); // assume armed if armed channel is not defined
armed = controlThrottle >= 0.05 && controlArmed >= 0.5;
// NOTE: put ACRO or MANUAL modes there if you want to use them
if (controls[modeChannel] < 0.25) {
if (controlMode < 0.25) {
mode = STAB;
} else if (controls[modeChannel] < 0.75) {
} else if (controlMode < 0.75) {
mode = STAB;
} else {
mode = STAB;
}
thrustTarget = controls[throttleChannel];
thrustTarget = controlThrottle;
if (mode == ACRO) {
yawMode = YAW_RATE;
ratesTarget.x = controls[rollChannel] * maxRate.x;
ratesTarget.y = controls[pitchChannel] * maxRate.y;
ratesTarget.z = -controls[yawChannel] * maxRate.z; // positive yaw stick means clockwise rotation in FLU
ratesTarget.x = controlRoll * maxRate.x;
ratesTarget.y = controlPitch* maxRate.y;
ratesTarget.z = -controlYaw * maxRate.z; // positive yaw stick means clockwise rotation in FLU
} else if (mode == STAB) {
yawMode = controls[yawChannel] == 0 ? YAW : YAW_RATE;
yawMode = controlYaw == 0 ? YAW : YAW_RATE;
attitudeTarget = Quaternion::fromEuler(Vector(
controls[rollChannel] * tiltMax,
controls[pitchChannel] * tiltMax,
controlRoll * tiltMax,
controlPitch * tiltMax,
attitudeTarget.getYaw()));
ratesTarget.z = -controls[yawChannel] * maxRate.z; // positive yaw stick means clockwise rotation in FLU
ratesTarget.z = -controlYaw * maxRate.z; // positive yaw stick means clockwise rotation in FLU
} else if (mode == MANUAL) {
// passthrough mode
yawMode = YAW_RATE;
torqueTarget = Vector(controls[rollChannel], controls[pitchChannel], -controls[yawChannel]) * 0.01;
torqueTarget = Vector(controlRoll, controlPitch, -controlYaw) * 0.01;
}
if (yawMode == YAW_RATE || !motorsActive()) {

18
flix/failsafe.ino
View File

@ -6,8 +6,8 @@
#define RC_LOSS_TIMEOUT 0.2
#define DESCEND_TIME 3.0 // time to descend from full throttle to zero
extern double controlsTime;
extern int rollChannel, pitchChannel, throttleChannel, yawChannel;
extern double controlTime;
extern float controlRoll, controlPitch, controlThrottle, controlYaw;
void failsafe() {
armingFailsafe();
@ -19,13 +19,13 @@ void armingFailsafe() {
static double zeroThrottleTime;
static double armingTime;
if (!armed) armingTime = t; // stores the last time when the drone was disarmed, therefore contains arming time
if (controlsTime > 0 && controls[throttleChannel] < 0.05) zeroThrottleTime = controlsTime;
if (controlTime > 0 && controlThrottle < 0.05) zeroThrottleTime = controlTime;
if (armingTime - zeroThrottleTime > 0.1) armed = false; // prevent arming if there was no zero throttle for 0.1 sec
}
// RC loss failsafe
void rcLossFailsafe() {
if (t - controlsTime > RC_LOSS_TIMEOUT) {
if (t - controlTime > RC_LOSS_TIMEOUT) {
descend();
}
}
@ -33,9 +33,9 @@ void rcLossFailsafe() {
// Smooth descend on RC lost
void descend() {
mode = STAB;
controls[rollChannel] = 0;
controls[pitchChannel] = 0;
controls[yawChannel] = 0;
controls[throttleChannel] -= dt / DESCEND_TIME;
if (controls[throttleChannel] < 0) controls[throttleChannel] = 0;
controlRoll = 0;
controlPitch = 0;
controlYaw = 0;
controlThrottle -= dt / DESCEND_TIME;
if (controlThrottle < 0) controlThrottle = 0;
}

3
flix/flix.ino
View File

@ -12,8 +12,7 @@
double t = NAN; // current step time, s
float dt; // time delta from previous step, s
int16_t channels[16]; // raw rc channels
float controls[16]; // normalized controls in range [-1..1] ([0..1] for throttle)
float controlRoll, controlPitch, controlYaw, controlThrottle, controlArmed, controlMode; // pilot's inputs, range [-1, 1]
Vector gyro; // gyroscope data
Vector acc; // accelerometer data, m/s/s
Vector rates; // filtered angular rates, rad/s

28
flix/mavlink.ino
View File

@ -15,8 +15,8 @@
float mavlinkControlScale = 0.7;
String mavlinkPrintBuffer;
extern double controlsTime;
extern int rollChannel, pitchChannel, throttleChannel, yawChannel, armedChannel, modeChannel;
extern double controlTime;
extern float controlRoll, controlPitch, controlThrottle, controlYaw, controlArmed, controlMode;
void processMavlink() {
sendMavlink();
@ -54,11 +54,9 @@ void sendMavlink() {
time, att.w, att.x, att.y, att.z, rates.x, rates.y, rates.z, zeroQuat);
sendMessage(&msg);
mavlink_msg_rc_channels_scaled_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg, controlsTime * 1000, 0,
controls[0] * 10000, controls[1] * 10000, controls[2] * 10000,
controls[3] * 10000, controls[4] * 10000, controls[5] * 10000,
INT16_MAX, INT16_MAX, UINT8_MAX);
sendMessage(&msg);
mavlink_msg_rc_channels_raw_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg, controlTime * 1000, 0,
channels[0], channels[1], channels[2], channels[3], channels[4], channels[5], channels[6], channels[7], UINT8_MAX);
if (channels[0] != 0) sendMessage(&msg); // 0 means no RC input
float actuator[32];
memcpy(actuator, motors, sizeof(motors));
@ -101,15 +99,15 @@ void handleMavlink(const void *_msg) {
mavlink_msg_manual_control_decode(&msg, &m);
if (m.target && m.target != SYSTEM_ID) return; // 0 is broadcast
controls[throttleChannel] = m.z / 1000.0f;
controls[pitchChannel] = m.x / 1000.0f * mavlinkControlScale;
controls[rollChannel] = m.y / 1000.0f * mavlinkControlScale;
controls[yawChannel] = m.r / 1000.0f * mavlinkControlScale;
controls[modeChannel] = 1; // STAB mode
controls[armedChannel] = 1; // armed
controlsTime = t;
controlThrottle = m.z / 1000.0f;
controlPitch = m.x / 1000.0f * mavlinkControlScale;
controlRoll = m.y / 1000.0f * mavlinkControlScale;
controlYaw = m.r / 1000.0f * mavlinkControlScale;
controlMode = 1; // STAB mode
controlArmed = 1; // armed
controlTime = t;
if (abs(controls[yawChannel]) < MAVLINK_CONTROL_YAW_DEAD_ZONE) controls[yawChannel] = 0;
if (abs(controlYaw) < MAVLINK_CONTROL_YAW_DEAD_ZONE) controlYaw = 0;
}
if (msg.msgid == MAVLINK_MSG_ID_PARAM_REQUEST_LIST) {

25
flix/parameters.ino
View File

@ -5,8 +5,9 @@
#include <Preferences.h>
extern float channelNeutral[16];
extern float channelZero[16];
extern float channelMax[16];
extern float rollChannel, pitchChannel, throttleChannel, yawChannel, armedChannel, modeChannel;
extern float mavlinkControlScale;
Preferences storage;
@ -49,14 +50,14 @@ Parameter parameters[] = {
{"ACC_SCALE_Y", &accScale.y},
{"ACC_SCALE_Z", &accScale.z},
// rc
{"RC_NEUTRAL_0", &channelNeutral[0]},
{"RC_NEUTRAL_1", &channelNeutral[1]},
{"RC_NEUTRAL_2", &channelNeutral[2]},
{"RC_NEUTRAL_3", &channelNeutral[3]},
{"RC_NEUTRAL_4", &channelNeutral[4]},
{"RC_NEUTRAL_5", &channelNeutral[5]},
{"RC_NEUTRAL_6", &channelNeutral[6]},
{"RC_NEUTRAL_7", &channelNeutral[7]},
{"RC_ZERO_0", &channelZero[0]},
{"RC_ZERO_1", &channelZero[1]},
{"RC_ZERO_2", &channelZero[2]},
{"RC_ZERO_3", &channelZero[3]},
{"RC_ZERO_4", &channelZero[4]},
{"RC_ZERO_5", &channelZero[5]},
{"RC_ZERO_6", &channelZero[6]},
{"RC_ZERO_7", &channelZero[7]},
{"RC_MAX_0", &channelMax[0]},
{"RC_MAX_1", &channelMax[1]},
{"RC_MAX_2", &channelMax[2]},
@ -65,6 +66,12 @@ Parameter parameters[] = {
{"RC_MAX_5", &channelMax[5]},
{"RC_MAX_6", &channelMax[6]},
{"RC_MAX_7", &channelMax[7]},
{"RC_ROLL", &rollChannel},
{"RC_PITCH", &pitchChannel},
{"RC_THROTTLE", &throttleChannel},
{"RC_YAW", &yawChannel},
{"RC_ARMED", &armedChannel},
{"RC_MODE", &modeChannel},
#if WIFI_ENABLED
// MAVLink
{"MAV_CTRL_SCALE", &mavlinkControlScale},

101
flix/rc.ino
View File

@ -8,17 +8,14 @@
SBUS RC(Serial2); // NOTE: Use RC(Serial2, 16, 17) if you use the old UART2 pins
// RC channels mapping:
int rollChannel = 0;
int pitchChannel = 1;
int throttleChannel = 2;
int yawChannel = 3;
int armedChannel = 4;
int modeChannel = 5;
uint16_t channels[16]; // raw rc channels
double controlTime; // time of the last controls update
float channelZero[16]; // calibration zero values
float channelMax[16]; // calibration max values
// Channels mapping (using float to store in parameters):
float rollChannel = NAN, pitchChannel = NAN, throttleChannel = NAN, yawChannel = NAN, armedChannel = NAN, modeChannel = NAN;
double controlsTime; // time of the last controls update
float channelNeutral[16] = {NAN}; // first element NAN means not calibrated
float channelMax[16];
void setupRC() {
print("Setup RC\n");
@ -28,42 +25,76 @@ void setupRC() {
bool readRC() {
if (RC.read()) {
SBUSData data = RC.data();
memcpy(channels, data.ch, sizeof(channels)); // copy channels data
for (int i = 0; i < 16; i++) channels[i] = data.ch[i]; // copy channels data
normalizeRC();
controlsTime = t;
controlTime = t;
return true;
}
return false;
}
void normalizeRC() {
if (isnan(channelNeutral[0])) return; // skip if not calibrated
for (uint8_t i = 0; i < 16; i++) {
controls[i] = mapf(channels[i], channelNeutral[i], channelMax[i], 0, 1);
float controls[16];
for (int i = 0; i < 16; i++) {
controls[i] = mapf(channels[i], channelZero[i], channelMax[i], 0, 1);
}
// Update control values
controlRoll = rollChannel >= 0 ? controls[(int)rollChannel] : NAN;
controlPitch = pitchChannel >= 0 ? controls[(int)pitchChannel] : NAN;
controlYaw = yawChannel >= 0 ? controls[(int)yawChannel] : NAN;
controlThrottle = throttleChannel >= 0 ? controls[(int)throttleChannel] : NAN;
controlArmed = armedChannel >= 0 ? controls[(int)armedChannel] : 1; // assume armed by default
controlMode = modeChannel >= 0 ? controls[(int)modeChannel] : NAN;
}
void calibrateRC() {
print("Calibrate RC: move all sticks to maximum positions [4 sec]\n");
print("··o ··o\n··· ···\n··· ···\n");
pause(4);
while (!readRC());
for (int i = 0; i < 16; i++) {
channelMax[i] = channels[i];
}
print("Calibrate RC: move all sticks to neutral positions [4 sec]\n");
print("··· ···\n··· ·o·\n·o· ···\n");
pause(4);
while (!readRC());
for (int i = 0; i < 16; i++) {
channelNeutral[i] = channels[i];
}
printRCCal();
uint16_t zero[16];
uint16_t center[16];
uint16_t max[16];
print("1/9 Calibrating RC: put all switches to default positions [3 sec]\n");
pause(3);
calibrateRCChannel(NULL, zero, zero, "2/9 Move sticks [3 sec]\n... ...\n... .o.\n.o. ...\n");
calibrateRCChannel(NULL, center, center, "3/9 Move sticks [3 sec]\n... ...\n.o. .o.\n... ...\n");
calibrateRCChannel(&throttleChannel, zero, max, "4/9 Move sticks [3 sec]\n.o. ...\n... .o.\n... ...\n");
calibrateRCChannel(&yawChannel, center, max, "5/9 Move sticks [3 sec]\n... ...\n..o .o.\n... ...\n");
calibrateRCChannel(&pitchChannel, zero, max, "6/9 Move sticks [3 sec]\n... .o.\n... ...\n.o. ...\n");
calibrateRCChannel(&rollChannel, zero, max, "7/9 Move sticks [3 sec]\n... ...\n... ..o\n.o. ...\n");
calibrateRCChannel(&armedChannel, zero, max, "8/9 Switch to armed [3 sec]\n");
calibrateRCChannel(&modeChannel, zero, max, "9/9 Disarm and switch mode to max [3 sec]\n");
printRCCalibration();
}
void printRCCal() {
for (int i = 0; i < sizeof(channelNeutral) / sizeof(channelNeutral[0]); i++) print("%g ", channelNeutral[i]);
print("\n");
for (int i = 0; i < sizeof(channelMax) / sizeof(channelMax[0]); i++) print("%g ", channelMax[i]);
print("\n");
void calibrateRCChannel(float *channel, uint16_t in[16], uint16_t out[16], const char *str) {
print("%s", str);
pause(3);
for (int i = 0; i < 30; i++) readRC(); // try update 30 times max
memcpy(out, channels, sizeof(channels));
if (channel == NULL) return; // no channel to calibrate
// Find channel that changed the most between in and out
int ch = -1, diff = 0;
for (int i = 0; i < 16; i++) {
if (abs(out[i] - in[i]) > diff) {
ch = i;
diff = abs(out[i] - in[i]);
}
}
if (ch >= 0 && diff > 10) { // difference threshold is 10
*channel = ch;
channelZero[ch] = in[ch];
channelMax[ch] = out[ch];
} else {
*channel = NAN;
}
}
void printRCCalibration() {
print("Control Ch Zero Max\n");
print("Roll %-7g%-7g%-7g\n", rollChannel, rollChannel >= 0 ? channelZero[(int)rollChannel] : NAN, rollChannel >= 0 ? channelMax[(int)rollChannel] : NAN);
print("Pitch %-7g%-7g%-7g\n", pitchChannel, pitchChannel >= 0 ? channelZero[(int)pitchChannel] : NAN, pitchChannel >= 0 ? channelMax[(int)pitchChannel] : NAN);
print("Yaw %-7g%-7g%-7g\n", yawChannel, yawChannel >= 0 ? channelZero[(int)yawChannel] : NAN, yawChannel >= 0 ? channelMax[(int)yawChannel] : NAN);
print("Throttle %-7g%-7g%-7g\n", throttleChannel, throttleChannel >= 0 ? channelZero[(int)throttleChannel] : NAN, throttleChannel >= 0 ? channelMax[(int)throttleChannel] : NAN);
print("Armed %-7g%-7g%-7g\n", armedChannel, armedChannel >= 0 ? channelZero[(int)armedChannel] : NAN, armedChannel >= 0 ? channelMax[(int)armedChannel] : NAN);
print("Mode %-7g%-7g%-7g\n", modeChannel, modeChannel >= 0 ? channelZero[(int)modeChannel] : NAN, modeChannel >= 0 ? channelMax[(int)modeChannel] : NAN);
}

3
gazebo/SBUS.h
View File

@ -18,6 +18,9 @@ public:
SBUSData data() {
SBUSData data;
joystickGet(data.ch);
for (int i = 0; i < 16; i++) {
data.ch[i] = map(data.ch[i], -32768, 32767, 1000, 2000); // convert to pulse width style
}
return data;
};
};

8
gazebo/flix.h
View File

@ -15,8 +15,7 @@
double t = NAN;
float dt;
float motors[4];
int16_t channels[16]; // raw rc channels
float controls[16];
float controlRoll, controlPitch, controlYaw, controlThrottle, controlArmed, controlMode;
Vector acc;
Vector gyro;
Vector rates;
@ -41,9 +40,10 @@ void print(const char* format, ...);
void pause(float duration);
void doCommand(String str, bool echo);
void handleInput();
void calibrateRC();
void normalizeRC();
void printRCCal();
void calibrateRC();
void calibrateRCChannel(float *channel, uint16_t zero[16], uint16_t max[16], const char *str);
void printRCCalibration();
void dumpLog();
void processMavlink();
void sendMavlink();

4
gazebo/simulator.cpp
View File

@ -71,11 +71,7 @@ public:
gyro = Vector(imu->AngularVelocity().X(), imu->AngularVelocity().Y(), imu->AngularVelocity().Z());
acc = this->accFilter.update(Vector(imu->LinearAcceleration().X(), imu->LinearAcceleration().Y(), imu->LinearAcceleration().Z()));
// read rc
readRC();
controls[modeChannel] = 1; // 0 acro, 1 stab
controls[armedChannel] = 1; // armed
estimate();
// correct yaw to the actual yaw