From 52819e403b83ce4a52821198e677712c2a216620 Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Mon, 14 Jul 2025 11:11:32 +0300 Subject: [PATCH] Major rework of rc subsystem 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 --- docs/book/firmware.md | 2 +- docs/firmware.md | 2 +- flix/cli.ino | 17 +++---- flix/control.ino | 29 ++++++------ flix/failsafe.ino | 18 ++++---- flix/flix.ino | 3 +- flix/mavlink.ino | 28 ++++++------ flix/parameters.ino | 25 +++++++---- flix/rc.ino | 101 +++++++++++++++++++++++++++--------------- gazebo/SBUS.h | 3 ++ gazebo/flix.h | 8 ++-- gazebo/simulator.cpp | 4 -- 12 files changed, 137 insertions(+), 103 deletions(-) diff --git a/docs/book/firmware.md b/docs/book/firmware.md index 6f88b60..f8bb47e 100644 --- a/docs/book/firmware.md +++ b/docs/book/firmware.md @@ -10,7 +10,7 @@ * `acc` *(Vector)* — данные с акселерометра, *м/с2*. * `rates` *(Vector)* — отфильтрованные угловые скорости, *рад/с*. * `attitude` *(Quaternion)* — оценка ориентации (положения) дрона. -* `controls` *(float[])* — команды управления от пилота, в диапазоне [-1, 1]. +* `controlRoll`, `controlPitch`, ... *(float[])* — команды управления от пилота, в диапазоне [-1, 1]. * `motors` *(float[])* — выходные сигналы на моторы, в диапазоне [0, 1]. ## Исходные файлы diff --git a/docs/firmware.md b/docs/firmware.md index 8dc9884..5b672cd 100644 --- a/docs/firmware.md +++ b/docs/firmware.md @@ -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/s2*. * `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 diff --git a/flix/cli.ino b/flix/cli.ino index f611106..afc602f 100644 --- a/flix/cli.ino +++ b/flix/cli.ino @@ -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]); diff --git a/flix/control.ino b/flix/control.ino index 0b3ff32..623e8fb 100644 --- a/flix/control.ino +++ b/flix/control.ino @@ -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()) { diff --git a/flix/failsafe.ino b/flix/failsafe.ino index b217994..0d9d646 100644 --- a/flix/failsafe.ino +++ b/flix/failsafe.ino @@ -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; } diff --git a/flix/flix.ino b/flix/flix.ino index da2aaa2..531ad18 100644 --- a/flix/flix.ino +++ b/flix/flix.ino @@ -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 diff --git a/flix/mavlink.ino b/flix/mavlink.ino index a38661b..c3cded8 100644 --- a/flix/mavlink.ino +++ b/flix/mavlink.ino @@ -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) { diff --git a/flix/parameters.ino b/flix/parameters.ino index 612fe34..793e15d 100644 --- a/flix/parameters.ino +++ b/flix/parameters.ino @@ -5,8 +5,9 @@ #include -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}, diff --git a/flix/rc.ino b/flix/rc.ino index e78cb42..678973a 100644 --- a/flix/rc.ino +++ b/flix/rc.ino @@ -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); } diff --git a/gazebo/SBUS.h b/gazebo/SBUS.h index f57db7f..eb6b2ec 100644 --- a/gazebo/SBUS.h +++ b/gazebo/SBUS.h @@ -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; }; }; diff --git a/gazebo/flix.h b/gazebo/flix.h index 035d160..2891303 100644 --- a/gazebo/flix.h +++ b/gazebo/flix.h @@ -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(); diff --git a/gazebo/simulator.cpp b/gazebo/simulator.cpp index bcfa4ef..316d2bf 100644 --- a/gazebo/simulator.cpp +++ b/gazebo/simulator.cpp @@ -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