aleks/flix
1
0
Fork 0
mirror of https://github.com/okalachev/flix.git synced 2026-01-12 05:57:52 +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
Download Patch File Download Diff File Expand all files Collapse all files

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);
}