mirror of
https://github.com/okalachev/flix.git
synced 2026-01-12 05:57:52 +00:00
8418723ccc
Add interpretControls function to convert pilot commands and mode into control targets and make control functions independent from the mode. Add ratesExtra target for rates feed-forward; remove yawMode. Rename controlRate to controlRates to reflect rates variable name. Remove USER mode.
100 lines
4.2 KiB
C++
100 lines
4.2 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); // NOTE: Use RC(Serial2, 16, 17) if you use the old UART2 pins
|
|
|
|
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;
|
|
|
|
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] : 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] : NAN;
|
|
controlMode = modeChannel >= 0 ? controls[(int)modeChannel] : NAN;
|
|
}
|
|
|
|
void calibrateRC() {
|
|
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 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);
|
|
}
|