mirror of
https://github.com/okalachev/flix.git
synced 2026-02-16 23:21:31 +00:00
99 lines
3.8 KiB
C++
99 lines
3.8 KiB
C++
// Copyright (c) 2023 Oleg Kalachev <okalachev@gmail.com>
|
|
// Repository: https://github.com/okalachev/flix
|
|
|
|
// Work with the RC receiver
|
|
|
|
#include <SBUS.h>
|
|
#include "util.h"
|
|
|
|
SBUS rc(Serial2);
|
|
|
|
uint16_t channels[16]; // raw rc channels
|
|
int channelZero[16]; // calibration zero values
|
|
int channelMax[16]; // calibration max values
|
|
|
|
float controlRoll, controlPitch, controlYaw, controlThrottle; // pilot's inputs, range [-1, 1]
|
|
float controlMode = NAN;
|
|
float controlTime = NAN; // time of the last controls update
|
|
|
|
int rollChannel = -1, pitchChannel = -1, throttleChannel = -1, yawChannel = -1, modeChannel = -1; // channel mapping
|
|
|
|
void setupRC() {
|
|
print("Setup RC\n");
|
|
rc.begin();
|
|
}
|
|
|
|
bool readRC() {
|
|
if (rc.read()) {
|
|
SBUSData data = rc.data();
|
|
for (int i = 0; i < 16; i++) channels[i] = data.ch[i]; // copy channels data
|
|
normalizeRC();
|
|
controlTime = t;
|
|
return true;
|
|
}
|
|
return false;
|
|
}
|
|
|
|
void normalizeRC() {
|
|
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 ? 0 : controls[rollChannel];
|
|
controlPitch = pitchChannel < 0 ? 0 : controls[pitchChannel];
|
|
controlYaw = yawChannel < 0 ? 0 : controls[yawChannel];
|
|
controlThrottle = throttleChannel < 0 ? 0 : controls[throttleChannel];
|
|
controlMode = modeChannel < 0 ? NAN : controls[modeChannel]; // mode control is ineffective if not mapped
|
|
}
|
|
|
|
void calibrateRC() {
|
|
uint16_t zero[16];
|
|
uint16_t center[16];
|
|
uint16_t max[16];
|
|
print("1/8 Calibrating RC: put all switches to default positions [3 sec]\n");
|
|
pause(3);
|
|
calibrateRCChannel(NULL, zero, zero, "2/8 Move sticks [3 sec]\n... ...\n... .o.\n.o. ...\n");
|
|
calibrateRCChannel(NULL, center, center, "3/8 Move sticks [3 sec]\n... ...\n.o. .o.\n... ...\n");
|
|
calibrateRCChannel(&throttleChannel, zero, max, "4/8 Move sticks [3 sec]\n.o. ...\n... .o.\n... ...\n");
|
|
calibrateRCChannel(&yawChannel, center, max, "5/8 Move sticks [3 sec]\n... ...\n..o .o.\n... ...\n");
|
|
calibrateRCChannel(&pitchChannel, zero, max, "6/8 Move sticks [3 sec]\n... .o.\n... ...\n.o. ...\n");
|
|
calibrateRCChannel(&rollChannel, zero, max, "7/8 Move sticks [3 sec]\n... ...\n... ..o\n.o. ...\n");
|
|
calibrateRCChannel(&modeChannel, zero, max, "8/8 Put mode switch to max [3 sec]\n");
|
|
printRCCalibration();
|
|
}
|
|
|
|
void calibrateRCChannel(int *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 = -1;
|
|
}
|
|
}
|
|
|
|
void printRCCalibration() {
|
|
print("Control Ch Zero Max\n");
|
|
print("Roll %-7d%-7d%-7d\n", rollChannel, rollChannel < 0 ? 0 : channelZero[rollChannel], rollChannel < 0 ? 0 : channelMax[rollChannel]);
|
|
print("Pitch %-7d%-7d%-7d\n", pitchChannel, pitchChannel < 0 ? 0 : channelZero[pitchChannel], pitchChannel < 0 ? 0 : channelMax[pitchChannel]);
|
|
print("Yaw %-7d%-7d%-7d\n", yawChannel, yawChannel < 0 ? 0 : channelZero[yawChannel], yawChannel < 0 ? 0 : channelMax[yawChannel]);
|
|
print("Throttle %-7d%-7d%-7d\n", throttleChannel, throttleChannel < 0 ? 0 : channelZero[throttleChannel], throttleChannel < 0 ? 0 : channelMax[throttleChannel]);
|
|
print("Mode %-7d%-7d%-7d\n", modeChannel, modeChannel < 0 ? 0 : channelZero[modeChannel], modeChannel < 0 ? 0 : channelMax[modeChannel]);
|
|
}
|