mirror of
https://github.com/okalachev/flix.git
synced 2026-01-12 05:57:52 +00:00
c8e5e08b03
As it makes the subsystems code easier to understand. Declare the most used variables in main sketch file as forward declarations. Make all control input zero by default (except controlMode). Minor changes.
100 lines
4.0 KiB
C++
100 lines
4.0 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
|
|
float channelZero[16]; // calibration zero values
|
|
float channelMax[16]; // calibration max values
|
|
|
|
float controlRoll, controlPitch, controlYaw, controlThrottle; // pilot's inputs, range [-1, 1]
|
|
float controlMode = NAN; //
|
|
float controlTime; // time of the last controls update (0 when no RC)
|
|
|
|
// Channels mapping (using float to store in parameters):
|
|
float rollChannel = NAN, pitchChannel = NAN, throttleChannel = NAN, yawChannel = NAN, modeChannel = NAN;
|
|
|
|
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 ? controls[(int)rollChannel] : 0;
|
|
controlPitch = pitchChannel >= 0 ? controls[(int)pitchChannel] : 0;
|
|
controlYaw = yawChannel >= 0 ? controls[(int)yawChannel] : 0;
|
|
controlThrottle = throttleChannel >= 0 ? controls[(int)throttleChannel] : 0;
|
|
controlMode = modeChannel >= 0 ? controls[(int)modeChannel] : NAN; // mode switch should not have affect if not set
|
|
}
|
|
|
|
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(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("Mode %-7g%-7g%-7g\n", modeChannel, modeChannel >= 0 ? channelZero[(int)modeChannel] : NAN, modeChannel >= 0 ? channelMax[(int)modeChannel] : NAN);
|
|
}
|